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)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c1[0]
marker.color.g = c1[1]
marker.color.b = c1[2]
marker.color.a = 1.
marker_pub.publish(marker)
marker.id = marker_id * 100 + 1
if arm == 'left_arm':
#rot2 = tr.Rz(math.radians(90.)) * rot.T
rot2 = rot * tr.rotZ(-math.pi / 2)
else:
#rot2 = tr.Rz(math.radians(-90.)) * rot.T
rot2 = rot * tr.rotZ(math.pi / 2)
quat = tr.matrix_to_quaternion(rot2)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c2[0]
marker.color.g = c2[1]
marker.color.b = c2[2]
marker.color.a = 1.
marker_pub.publish(marker)
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)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c1[0]
marker.color.g = c1[1]
marker.color.b = c1[2]
marker.color.a = 1.
marker_pub.publish(marker)
marker.id = marker_id*100 + 1
if arm == 'left_arm':
#rot2 = tr.Rz(math.radians(90.)) * rot.T
rot2 = rot * tr.rotZ(-math.pi/2)
else:
#rot2 = tr.Rz(math.radians(-90.)) * rot.T
rot2 = rot * tr.rotZ(math.pi/2)
quat = tr.matrix_to_quaternion(rot2)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c2[0]
marker.color.g = c2[1]
marker.color.b = c2[2]
marker.color.a = 1.
marker_pub.publish(marker)
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)
def step_ros(self):
r_arm = 'right_arm'
l_arm = 'left_arm'
self.cb_lock.acquire()
r_jep = copy.copy(self.r_jep)
l_jep = copy.copy(self.l_jep)
r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)
l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)
self.cb_lock.release()
self.set_jep(r_arm, r_jep)
self.set_jep(l_arm, l_jep)
self.set_alpha(r_arm, r_alpha)
self.set_alpha(l_arm, l_alpha)
self.step()
motor_pwr_state = self.is_motor_power_on()
if not motor_pwr_state:
self.maintain_configuration()
q_r = self.get_joint_angles(r_arm)
q_l = self.get_joint_angles(l_arm)
if self.floating_arms:
if self.qr_prev == None or self.floating_arms_counter < 100:
self.qr_prev = q_r
self.ql_prev = q_l
self.floating_arms_counter += 1
else:
tol = np.radians([5., 2., 10., 2., 10., 0.03, 0.6])
r_arr = np.array(q_r)
l_arr = np.array(q_l)
prev_r_arr = np.array(self.qr_prev)
prev_l_arr = np.array(self.ql_prev)
dqr = np.array(q_r) - np.array(prev_r_arr)
dql = np.array(q_l) - np.array(prev_l_arr)
dqr = dqr * (np.abs(dqr) > tol)
dql = dql * (np.abs(dql) > tol)
r_jep = (np.array(r_jep) + dqr).tolist()
l_jep = (np.array(l_jep) + dql).tolist()
self.cb_lock.acquire()
self.r_jep = copy.copy(r_jep)
self.l_jep = copy.copy(l_jep)
self.cb_lock.release()
change_idxs = np.where(dqr != 0)
prev_r_arr[change_idxs] = r_arr[change_idxs]
change_idxs = np.where(dql != 0)
prev_l_arr[change_idxs] = l_arr[change_idxs]
self.qr_prev = prev_r_arr.tolist()
self.ql_prev = prev_l_arr.tolist()
f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
f_r = self.xhat_force[r_arm]
f_l = self.xhat_force[l_arm]
# publish stuff over ROS.
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
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))
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)
def step_ros(self):
r_arm = 'right_arm'
l_arm = 'left_arm'
self.cb_lock.acquire()
r_jep = copy.copy(self.r_jep)
l_jep = copy.copy(self.l_jep)
r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)
l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)
self.cb_lock.release()
self.set_jep(r_arm, r_jep)
self.set_jep(l_arm, l_jep)
self.set_alpha(r_arm, r_alpha)
self.set_alpha(l_arm, l_alpha)
self.step()
motor_pwr_state = self.is_motor_power_on()
if not motor_pwr_state:
self.maintain_configuration()
q_r = self.get_joint_angles(r_arm)
q_l = self.get_joint_angles(l_arm)
if self.floating_arms:
if self.qr_prev == None or self.floating_arms_counter < 100:
self.qr_prev = q_r
self.ql_prev = q_l
self.floating_arms_counter += 1
else:
tol = np.radians([5., 2., 10., 2., 10., 0.03, 0.6])
r_arr = np.array(q_r)
l_arr = np.array(q_l)
prev_r_arr = np.array(self.qr_prev)
prev_l_arr = np.array(self.ql_prev)
dqr = np.array(q_r) - np.array(prev_r_arr)
dql = np.array(q_l) - np.array(prev_l_arr)
dqr = dqr * (np.abs(dqr) > tol)
dql = dql * (np.abs(dql) > tol)
r_jep = (np.array(r_jep) + dqr).tolist()
l_jep = (np.array(l_jep) + dql).tolist()
self.cb_lock.acquire()
self.r_jep = copy.copy(r_jep)
self.l_jep = copy.copy(l_jep)
self.cb_lock.release()
change_idxs = np.where(dqr != 0)
prev_r_arr[change_idxs] = r_arr[change_idxs]
change_idxs = np.where(dql != 0)
prev_l_arr[change_idxs] = l_arr[change_idxs]
self.qr_prev = prev_r_arr.tolist()
self.ql_prev = prev_l_arr.tolist()
f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
f_r = self.xhat_force[r_arm]
f_l = self.xhat_force[l_arm]
# publish stuff over ROS.
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
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))
