def set_jep(self, arm, q, duration=None): if arm == 'right_arm': pub = self.r_jep_cmd_pub elif arm == 'left_arm': pub = self.l_jep_cmd_pub else: raise RuntimeError('Undefined arm: %s'%(arm)) time_stamp = rospy.Time.now() h = Header() h.stamp = time_stamp pub.publish(FloatArray(h, q))
def set_impedance_scale(self, arm, s): if arm == 'right_arm': pub = self.r_alph_cmd_pub elif arm == 'left_arm': pub = self.l_alph_cmd_pub else: raise RuntimeError('Undefined arm: %s'%(arm)) time_stamp = rospy.Time.now() h = Header() h.stamp = time_stamp pub.publish(FloatArray(h, s))
def set_jep(self, arm, q, duration=None): if arm == 'right_arm': pub = self.r_jep_cmd_pub elif arm == 'left_arm': pub = self.l_jep_cmd_pub else: raise RuntimeError('Undefined arm: %s' % (arm)) time_stamp = rospy.Time.now() h = Header() h.stamp = time_stamp pub.publish(FloatArray(h, q))
def set_impedance_scale(self, arm, s): if arm == 'right_arm': pub = self.r_alph_cmd_pub elif arm == 'left_arm': pub = self.l_alph_cmd_pub else: raise RuntimeError('Undefined arm: %s' % (arm)) time_stamp = rospy.Time.now() h = Header() h.stamp = time_stamp pub.publish(FloatArray(h, s))
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))
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] 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)
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))