Update to urdf and watcher to make movement nice

This commit is contained in:
Dave Niewinski 2024-01-09 15:13:42 +00:00
parent aeb6d00f86
commit 8be6e31607
2 changed files with 46 additions and 11 deletions

View File

@ -7,13 +7,41 @@ import tf
import numpy as np
from zed_interfaces.msg import ObjectsStamped
import math
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
import copy
HEAD_DISTANCE = 0.6
limits = [ [-1.57, 1.57], [1.76, 2.50], [-1.76, -0.50], [0.90, 0.90], [-0.10, 0.10], [-3.14, 3.14] ]
limits = [ [-1.57, 1.57], [1.76, 2.50], [-1.76, -0.50], [-0.90, -0.90], [-0.10, 0.10], [-3.14, 3.14] ]
mount = [0, 1.759292, -1.700000, -0.9, 0, 0]
joint_names = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
def arm_command(pos, time):
msg = JointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.joint_names = joint_names
point_msg = JointTrajectoryPoint()
point_msg.time_from_start = rospy.Time.from_sec(time)
point_msg.positions = pos
msg.points = []
msg.points.append(point_msg)
arm_pub.publish(msg)
def j_limits(values):
j_values = []
for i in range(len(values)):
min_val = limits[i][0]
max_val = limits[i][1]
j_values.append(max(min(max_val, values[i]), min_val))
return j_values
def det_callback(msg):
global broadcaster, world_to_cam, view_to_world, world_to_view, last_pos
global broadcaster, world_to_cam, view_to_world, world_to_view
if len(msg.objects) > 0:
min_distance = 99999.0
@ -25,7 +53,7 @@ def det_callback(msg):
closest_obj = o
min_distance = length
o_mtx = compose_matrix(translate=[o.position[0], o.position[1], o.position[2]])
o_mtx = compose_matrix(translate=[o.position[0], o.position[1], o.bounding_box_3d.corners[0].kp[2]])
world_to_o = np.matmul(world_to_cam, o_mtx)
view_to_o = np.matmul(view_to_world, world_to_o)
scale, shear, angles, translate, perspective = decompose_matrix(view_to_o)
@ -36,6 +64,15 @@ def det_callback(msg):
h_mtx = compose_matrix(translate=[HEAD_DISTANCE, 0, 0])
t_mtx = compose_matrix(angles=[0,pitch,yaw])
j_angles = copy.deepcopy(mount)
j_angles[0] = yaw + 0.25 * yaw
j_angles[2] = j_angles[2] + pitch + 0.85
j_angles[4] = -0.25 * yaw
j_angles[5] = yaw/2.0
j_angles = j_limits(j_angles)
arm_command(j_angles, 0.5)
t_mtx = np.matmul(t_mtx, h_mtx)
t_mtx = np.matmul(world_to_view, t_mtx)
@ -49,7 +86,7 @@ def det_callback(msg):
"world")
def main():
global broadcaster, world_to_cam, view_to_world, world_to_view, last_pos
global broadcaster, world_to_cam, view_to_world, world_to_view, arm_pub
rospy.init_node('glados_watcher')
broadcaster = tf.TransformBroadcaster()
@ -64,14 +101,12 @@ def main():
world_to_view = compose_matrix(translate=trans, angles=euler_from_quaternion(rot))
view_to_world = np.linalg.inv(world_to_view)
# listener.waitForTransform('world', 'eye_link', rospy.Time(0), rospy.Duration(0.5))
# trans, rot = listener.lookupTransform('world', 'eye_link', rospy.Time(0))
# last_pos = compose_matrix(translate=trans, angles=euler_from_quaternion(rot))
# rospy.loginfo(last_pos)
arm_pub = rospy.Publisher('/z1_joint_traj_controller/command', JointTrajectory, queue_size=10)
rospy.Subscriber("/zed_node/obj_det/objects", ObjectsStamped, det_callback)
arm_command(mount, 2.0)
rospy.sleep(2.0)
rospy.spin()
if __name__ == '__main__':

View File

@ -77,7 +77,7 @@
<link name="view_link" />
<joint name="view_joint" type="fixed">
<origin xyz="0 0 0.4" rpy="0 0 0" />
<origin xyz="0 0 0.55" rpy="0 0 0" />
<parent link="world"/>
<child link="view_link"/>
</joint>