def main(): global openhmd_state, occulus_state, openhmd_state_valid, occulus_state_valid rospy.init_node('occulus_view') openhmd_sub = rospy.Subscriber("/openhmd/pose", Pose, openhmd_cb) occulus_sub = rospy.Subscriber("/ambf/env/openhmd/State", ObjectState, occulus_cb, queue_size=1) occulus_pub = rospy.Publisher("/ambf/env/openhmd/Command", ObjectCmd, queue_size=1) rate = rospy.Rate(60) counter = 0 start = rospy.get_time() _first = True openhmd_initial_rot = Rotation() occulus_initial_rot = Rotation() R_pre = Rotation() R_aInr_offset = Rotation().RPY(0, -1.57079, -1.57079) # open while not rospy.is_shutdown(): if openhmd_state_valid and occulus_state_valid: if _first: _first = False openhmd_initial_rot = Rotation.Quaternion(openhmd_state.orientation.x, openhmd_state.orientation.y, openhmd_state.orientation.z, openhmd_state.orientation.w) occulus_initial_rot = Rotation.Quaternion(occulus_state.pose.orientation.x, occulus_state.pose.orientation.y, occulus_state.pose.orientation.z, occulus_state.pose.orientation.w) R_pre = openhmd_initial_rot * R_aInr_offset * occulus_initial_rot.Inverse() else: occulus_cmd.pose.position = occulus_state.pose.position openhmd_rot = Rotation.Quaternion(openhmd_state.orientation.x, openhmd_state.orientation.y, openhmd_state.orientation.z, openhmd_state.orientation.w) delta_rot = R_pre.Inverse() * openhmd_rot * R_aInr_offset # delta_rot = openhmd_rot occulus_cmd.pose.orientation.x = delta_rot.GetQuaternion()[0] occulus_cmd.pose.orientation.y = delta_rot.GetQuaternion()[1] occulus_cmd.pose.orientation.z = delta_rot.GetQuaternion()[2] occulus_cmd.pose.orientation.w = delta_rot.GetQuaternion()[3] occulus_cmd.enable_position_controller = True occulus_pub.publish(occulus_cmd) counter = counter + 1 if counter % 60 == 0: print "- Publishing Occulus Pose ", format( round(rospy.get_time() - start, 3)), 's' rate.sleep()
class TaskToJointSpace: def __init__(self, arm_name='MTMR'): self._num_rows = 6 self._num_cols = 7 self._jac_spa = np.zeros((self._num_rows, self._num_cols)) self._jac_bod = np.zeros((self._num_rows, self._num_cols)) self._new_data = 0 self._rate = rospy.Rate(500) self._joint_state = JointState() self._cart_state = JointState() self._state_cmd = JointState() self._wrench_cmd = np.zeros((6, 1)) self._joint_torques_cmd = np.zeros((7, 1)) self._jac_spa_sub = rospy.Subscriber('/dvrk/' + arm_name + '/jacobian_spatial', Float64MultiArray, self.jac_spa_cb, queue_size=1) self._jac_bod_sub = rospy.Subscriber('/dvrk/' + arm_name + '/jacobian_body', Float64MultiArray, self.jac_bod_cb, queue_size=1) self._js_sub = rospy.Subscriber('/dvrk/' + arm_name + '/state_joint_current', JointState, self.js_cb, queue_size=1) self._cs_sub = rospy.Subscriber('/dvrk/' + arm_name + '/position_cartesian_current', PoseStamped, self.cs_cb, queue_size=1) self._wrench_bod_sub = rospy.Subscriber('/ambf/' + arm_name + '/set_wrench_body', Wrench, self.wrench_bod_cb, queue_size=1) self._torque_pub = rospy.Publisher('/dvrk/' + arm_name + '/set_effort_joint', JointState, queue_size=5) self._wd = WatchDog(0.1) self.l4_o = np.identity(3) self.l5_o = np.mat([[0, 0, -1], [0, 1, 0], [1, 0, 0]]) self.l6_o = np.mat([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]) self.l7_o = np.mat([[1, 0, 0], [0, 0, -1], [0, 1, 0]]) self.Kp = 0.3 self.Kd = 0 theta = 0 self._rot_offset = np.mat([[+1.0, +0.0, +0.0], [+0.0, +np.cos(theta), +np.sin(theta)], [+0.0, -np.sin(theta), +np.cos(theta)]]) self._ee_rot = Rotation() def set_k_gain(self, val): self.Kp = val def set_d_gain(self, val): self.Kd = val def jac_spa_cb(self, msg): self._new_data = True for r in range(0, self._num_rows): for c in range(0, self._num_cols): self._jac_spa[r][c] = msg.data[r + (6 * c)] pass def jac_bod_cb(self, msg): for r in range(0, self._num_rows): for c in range(0, self._num_cols): self._jac_bod[r][c] = msg.data[r + (6 * c)] pass def js_cb(self, msg): self._joint_state = msg def cs_cb(self, msg): self._cart_state = msg self._ee_rot = Rotation.Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w) def wrench_bod_cb(self, msg): force = np.zeros((3, 1)) force[0] = msg.force.x force[1] = msg.force.y force[2] = msg.force.z force = np.matmul(self._rot_offset, force) rel_force = Vector(force[0], force[1], force[2]) abs_force = self._ee_rot.Inverse() * rel_force print abs_force[0], abs_force[1], abs_force[2] self._wrench_cmd[0] = abs_force[0] self._wrench_cmd[1] = abs_force[1] self._wrench_cmd[2] = abs_force[2] self._wrench_cmd[3] = msg.torque.x self._wrench_cmd[4] = msg.torque.y self._wrench_cmd[5] = msg.torque.z self._wd.acknowledge_wd() def clear_wrench(self): self._wrench_cmd[0] = 0 self._wrench_cmd[1] = 0 self._wrench_cmd[2] = 0 self._wrench_cmd[3] = 0 self._wrench_cmd[4] = 0 self._wrench_cmd[5] = 0 def print_jacobian_spatial(self): jac = np.zeros(self._jac_spa.shape) for r in range(0, self._num_rows): for c in range(0, self._num_cols): jac[r][c] = round(self._jac_spa[r][c], 3) print(jac) def print_jacobian_body(self): jac = np.zeros(self._jac_bod.shape) for r in range(0, self._num_rows): for c in range(0, self._num_cols): jac[r][c] = round(self._jac_bod[r][c], 3) print(jac) def convert_force_to_torque_spa(self, force): torque = np.matmul(self._jac_spa.transpose(), force) return torque def convert_force_to_torque_bod(self, force): torque = np.matmul(self._jac_bod.transpose(), force) return torque def command_torques(self, torques): self._state_cmd.effort = torques.flatten().tolist() self._torque_pub.publish(self._state_cmd) def run(self): while not rospy.is_shutdown(): self._joint_torques_cmd = self.convert_force_to_torque_bod( self._wrench_cmd) r4 = rot_z(self._joint_state.position[3]) r5 = rot_z(self._joint_state.position[4]) r6 = rot_z(self._joint_state.position[5]) r7 = rot_z(self._joint_state.position[6]) re = self.l4_o * r4 * self.l5_o * r5 * self.l6_o * r6 * self.l7_o * r7 ve = re[:, 2] v4 = r4[:, 0] e = (np.pi / 2) - np.arccos(np.dot(v4.transpose(), ve)) tau4 = self.Kp * e - self.Kd * self._joint_state.velocity[3] np.clip(tau4, -0.3, 0.3) # self._joint_torques_cmd[3] = tau4[0, 0] self._joint_torques_cmd[3] = 0 self._joint_torques_cmd[4] = 0 self._joint_torques_cmd[5] = 0 self._joint_torques_cmd[6] = 0 # Apply the relation between J2 and J3 self._joint_torques_cmd[1] = self._joint_torques_cmd[1] # self._joint_torques_cmd[1] = -self._joint_torques_cmd[1] self._joint_torques_cmd[2] = self._joint_torques_cmd[2] self.command_torques(self._joint_torques_cmd) if self._wd.is_wd_expired(): self.clear_wrench() self._rate.sleep()