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()
