How to use the arms.link_tf_name function in arms
To help you get started, we’ve selected a few arms examples, based on popular ways it is used in public projects.
Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
gt-ros-pkg / hrl / hrl / hrl_arm_control / hrl_cody_arms / src / hrl_cody_arms / deprecated / arm_viz.py View on Github 
arms = ar.M3HrlRobot()
arm_client = ac.MekaArmClient(arms)
force_r_pub = rospy.Publisher('/r_arm/force_base', FloatArray)
force_l_pub = rospy.Publisher('/l_arm/force_base', FloatArray)
marker_pub = rospy.Publisher('/cody_arms/viz_marker', Marker)
rospy.logout('Sleeping ...')
rospy.sleep(1.0)
rospy.logout('... begin')
r_arm = 'right_arm'
l_arm = 'left_arm'
transform_bcast = tfb.TransformBroadcaster()
torso_link_name = ar.link_tf_name(r_arm, 0)
while not rospy.is_shutdown():
rospy.sleep(0.1)
f_r = arm_client.get_wrist_force(r_arm, base_frame=True)
f_l = arm_client.get_wrist_force(l_arm, base_frame=True)
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
force_r_pub.publish(FloatArray(h, f_r))
force_l_pub.publish(FloatArray(h, f_l))
publish_sphere_marker((0.5,0.5,0.5), 0.08, torso_link_name,
time_stamp, 'both_arms', 0)
for arm in [r_arm, l_arm]:
q = arm_client.get_joint_angles(arm)
links = [2, 3, 7]def publish_cartesian_markers(arm, time_stamp, cep, rot, c1, c2, marker_id):
marker = Marker()
marker.header.frame_id = ar.link_tf_name(arm, 0)
marker.header.stamp = time_stamp
marker.ns = arm
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose.position.x = cep[0,0]
marker.pose.position.y = cep[1,0]
marker.pose.position.z = cep[2,0]
marker.scale.x = 0.1
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.lifetime = rospy.Duration()
marker.id = marker_id*100 + 0
#rot1 = tr.Ry(math.radians(90.)) * rot.T
rot1 = rot * tr.rotY(math.pi/2)
quat = tr.matrix_to_quaternion(rot1)arms = ar.M3HrlRobot()
arm_client = ac.MekaArmClient(arms)
force_r_pub = rospy.Publisher('/r_arm/force_base', FloatArray)
force_l_pub = rospy.Publisher('/l_arm/force_base', FloatArray)
marker_pub = rospy.Publisher('/cody_arms/viz_marker', Marker)
rospy.logout('Sleeping ...')
rospy.sleep(1.0)
rospy.logout('... begin')
r_arm = 'right_arm'
l_arm = 'left_arm'
transform_bcast = tfb.TransformBroadcaster()
torso_link_name = ar.link_tf_name(r_arm, 0)
while not rospy.is_shutdown():
rospy.sleep(0.1)
f_r = arm_client.get_wrist_force(r_arm, base_frame=True)
f_l = arm_client.get_wrist_force(l_arm, base_frame=True)
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
force_r_pub.publish(FloatArray(h, f_r))
force_l_pub.publish(FloatArray(h, f_l))
publish_sphere_marker((0.5,0.5,0.5), 0.08, torso_link_name,
time_stamp, 'both_arms', 0)
for arm in [r_arm, l_arm]:
q = arm_client.get_joint_angles(arm)
links = [2, 3, 7]time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
force_r_pub.publish(FloatArray(h, f_r))
force_l_pub.publish(FloatArray(h, f_l))
publish_sphere_marker((0.5,0.5,0.5), 0.08, torso_link_name,
time_stamp, 'both_arms', 0)
for arm in [r_arm, l_arm]:
q = arm_client.get_joint_angles(arm)
links = [2, 3, 7]
for i in links:
p, rot = arms.FK_all(arm, q, i)
qaut = tr.matrix_to_quaternion(rot)
frameid = ar.link_tf_name(arm, i)
transform_bcast.sendTransform(p.A1.tolist(), qaut, time_stamp,
frameid, torso_link_name)
publish_sphere_marker((0.5,0.1,0.5), 0.05, frameid,
time_stamp, arm, i)
c1 = (0.5, 0.1, 0.5)
c2 = (0.5, 0.5, 0.1)
p, rot = arms.FK_all(arm, q)
publish_cartesian_markers(arm, time_stamp, p, rot, c1, c2,
marker_id=76)
c1 = (0.2, 0.2, 0.2)
c2 = (0.6, 0.6, 0.6)
jep = arm_client.get_jep(arm)
jep = arms.clamp_to_physical_joint_limits(arm, jep)
cep, rot = arms.FK_all(arm, jep)self.q_r_pub.publish(FloatArray(h, q_r))
self.q_l_pub.publish(FloatArray(h, q_l))
self.jep_r_pub.publish(FloatArray(h, r_jep))
self.jep_l_pub.publish(FloatArray(h, l_jep))
self.alph_r_pub.publish(FloatArray(h, r_alpha))
self.alph_l_pub.publish(FloatArray(h, l_alpha))
h.frame_id = '/torso_lift_link'
nms = self.joint_names_list['right_arm'] + self.joint_names_list['left_arm']
pos = q_r + q_l
self.joint_state_pub.publish(JointState(h, nms, pos,
[0.]*len(pos), [0.]*len(pos)))
h.frame_id = ar.link_tf_name(r_arm, 7)
self.force_raw_r_pub.publish(FloatArray(h, f_raw_r))
self.force_r_pub.publish(FloatArray(h, f_r))
h.frame_id = ar.link_tf_name(l_arm, 7)
self.force_raw_l_pub.publish(FloatArray(h, f_raw_l))
self.force_l_pub.publish(FloatArray(h, f_l))
self.pwr_state_pub.publish(Bool(motor_pwr_state))time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
force_r_pub.publish(FloatArray(h, f_r))
force_l_pub.publish(FloatArray(h, f_l))
publish_sphere_marker((0.5,0.5,0.5), 0.08, torso_link_name,
time_stamp, 'both_arms', 0)
for arm in [r_arm, l_arm]:
q = arm_client.get_joint_angles(arm)
links = [2, 3, 7]
for i in links:
p, rot = arms.FK_all(arm, q, i)
qaut = tr.matrix_to_quaternion(rot)
frameid = ar.link_tf_name(arm, i)
transform_bcast.sendTransform(p.A1.tolist(), qaut, time_stamp,
frameid, torso_link_name)
publish_sphere_marker((0.5,0.1,0.5), 0.05, frameid,
time_stamp, arm, i)
c1 = (0.5, 0.1, 0.5)
c2 = (0.5, 0.5, 0.1)
p, rot = arms.FK_all(arm, q)
publish_cartesian_markers(arm, time_stamp, p, rot, c1, c2,
marker_id=76)
c1 = (0.2, 0.2, 0.2)
c2 = (0.6, 0.6, 0.6)
jep = arm_client.get_jep(arm)
jep = arms.clamp_to_physical_joint_limits(arm, jep)
cep, rot = arms.FK_all(arm, jep)def publish_cartesian_markers(arm, time_stamp, cep, rot, c1, c2, marker_id):
marker = Marker()
marker.header.frame_id = ar.link_tf_name(arm, 0)
marker.header.stamp = time_stamp
marker.ns = arm
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose.position.x = cep[0,0]
marker.pose.position.y = cep[1,0]
marker.pose.position.z = cep[2,0]
marker.scale.x = 0.1
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.lifetime = rospy.Duration()
marker.id = marker_id*100 + 0
#rot1 = tr.Ry(math.radians(90.)) * rot.T
rot1 = rot * tr.rotY(math.pi/2)
quat = tr.matrix_to_quaternion(rot1)time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
force_r_pub.publish(FloatArray(h, f_r))
force_l_pub.publish(FloatArray(h, f_l))
publish_sphere_marker((0.5,0.5,0.5), 0.08, torso_link_name,
time_stamp, 'both_arms', 0)
for arm in [r_arm, l_arm]:
q = arm_client.get_joint_angles(arm)
links = [2, 3, 7]
for i in links:
p, rot = arms.FK_all(arm, q, i)
qaut = tr.matrix_to_quaternion(rot)
frameid = ar.link_tf_name(arm, i)
transform_bcast.sendTransform(p.A1.tolist(), qaut, time_stamp,
frameid, torso_link_name)
publish_sphere_marker((0.5,0.1,0.5), 0.05, frameid,
time_stamp, arm, i)
c1 = (0.5, 0.1, 0.5)
c2 = (0.5, 0.5, 0.1)
p, rot = arms.FK_all(arm, q)
publish_cartesian_markers(arm, time_stamp, p, rot, c1, c2,
marker_id=76)
c1 = (0.2, 0.2, 0.2)
c2 = (0.6, 0.6, 0.6)
jep = arm_client.get_jep(arm)
jep = arms.clamp_to_physical_joint_limits(arm, jep)
cep, rot = arms.FK_all(arm, jep)arms
CI/CD tool of Chongqing Parsec Corp.
MIT
Latest version published 2 years ago