def rot_matrix_from_vecs(vec_a, vec_b):
out = Rotation()
vec_a.Normalize()
vec_b.Normalize()
vcross = vec_a * vec_b
vdot = dot(vec_a, vec_b)
# Check if the vectors are in the same direction
if 1.0 - vdot < 0.1:
return out
# Or in the opposite direction
elif 1.0 + vdot < 0.1:
nx = Vector(1, 0, 0)
temp_dot = dot(vec_a, nx)
if -0.9 < abs(temp_dot) < 0.9:
axis = vec_a * nx
out = out.Rot(axis, 3.14)
else:
ny = Vector(0, 1, 0)
axis = vec_a * ny
out = out.Rot(axis, 3.14)
else:
skew_v = skew_mat(vcross)
out = add_mat(
add_mat(Rotation(), skew_v),
scalar_mul(skew_v * skew_v, (1 - vdot) / (vcross.Norm()**2)))
return out
def makePoses(self):
'''
产生位姿矩阵序列
'''
poses = []
points = self.makePoints()
for i in range(len(points)):
UX = Vector(points[i][0][0], points[i][0][1], points[i][0][2])
UY = Vector(points[i][1][0], points[i][1][1], points[i][1][2])
UZ = Vector(points[i][2][0], points[i][2][1], points[i][2][2])
Rot = Rotation(UX, UY, UZ)
pose = Pose()
pose.position.x = points[i][3][0]
pose.position.y = points[i][3][1]
pose.position.z = points[i][3][2]
q = Rot.GetQuaternion()
pose.orientation.x = q[0]
pose.orientation.y = q[1]
pose.orientation.z = q[2]
pose.orientation.w = q[3]
poses.append(pose)
return poses
def vel_calc_func(start_mat, end_mat, corrected_average_interval):
# Create rotation matrices
start_r = start_mat.M
end_r = end_mat.M
# Transform matrices using inverse for correct orientation
temp = start_mat.M.Inverse() * end_mat.M
temp_mat = Frame(Rotation(temp))
ang, temp_axis = Rotation.GetRotAngle(temp)
o = start_mat.M * temp_axis
p_start_vec = start_mat.p
p_end_vec = end_mat.p
# print "p vectors ", p_end_vec, p_start_vec
delta_x = p_end_vec[0] - p_start_vec[0]
print "delta x is ", delta_x
delta_y = p_end_vec[1] - p_start_vec[1]
delta_z = p_end_vec[2] - p_start_vec[2]
# Assign values to a Vector3 element
twist_vel = geometry_msgs.msg.Vector3()
twist_vel.x = delta_x / corrected_average_interval.to_sec()
twist_vel.y = delta_y / corrected_average_interval.to_sec()
twist_vel.z = delta_z / corrected_average_interval.to_sec()
# twist_rot = geometry_msgs.msg.Vector3()
twist_rot = o * (ang / corrected_average_interval.to_sec())
print "twist vel is ", twist_vel
print "twist rot is ", twist_rot[2]
return twist_vel, twist_rot
def __init__(self, name, mtm_name):
pose_str = name + 'pose'
button_str = name + 'button'
force_str = name + 'force_feedback'
self._active = False
self._scale = 0.0009
self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.base_frame = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.tip_frame = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.grey_button_pressed = False # Used as Position Engage Clutch
self.white_button_pressed = False # Used as Gripper Open Close Binary Angle
self._force = DeviceFeedback()
self._force.force.x = 0
self._force.force.y = 0
self._force.force.z = 0
self._force.position.x = 0
self._force.position.y = 0
self._force.position.z = 0
self._pose_sub = rospy.Subscriber(pose_str, PoseStamped, self.pose_cb, queue_size=10)
self._button_sub = rospy.Subscriber(button_str, DeviceButtonEvent, self.buttons_cb, queue_size=10)
self._force_pub = rospy.Publisher(force_str, DeviceFeedback, queue_size=1)
# External Clutch Buttons Requested by ICL
extra_footPedal_str = '/footPedal'
self._extra_pedal_cb = rospy.Subscriber(extra_footPedal_str, Vector3, self.extra_footpedal_cb, queue_size=1)
self._engageMTM = True
self._externalFootPedalMsg = Vector3()
self._geomagicButtonMsg = DeviceButtonEvent()
print('BINDING GEOMAGIC DEVICE: ', name, 'TO MOCK MTM DEVICE: ', mtm_name)
self._mtm_handle = ProxyMTM(mtm_name)
self._msg_counter = 0
def test_ik(x, y, z, rx, ry, rz):
Rx = Rotation.RPY(rx, 0.0, 0.0)
Ry = Rotation.RPY(0.0, ry, 0.0)
Rz = Rotation.RPY(0.0, 0.0, rz)
tip_offset_rot = Rotation.RPY(np.pi, 0, np.pi/2)
req_rot = tip_offset_rot * Rz * Ry * Rx
req_pos = Vector(x, y, z)
T_7_0 = Frame(req_rot, req_pos)
# print "REQ POSE \n", round_transform(convert_frame_to_mat(T_7_0), 3), "\n\n--------\n\n"
compute_IK(T_7_0)
def runOneArm(self, arm):
arm.obj_gui.App.update()
# Move the Target Position Based on the GUI
x = arm.obj_gui.x
y = arm.obj_gui.y
z = arm.obj_gui.z
ro = arm.obj_gui.ro
pi = arm.obj_gui.pi
ya = arm.obj_gui.ya
gr = arm.obj_gui.gr
arm.target.set_pos(x, y, z)
arm.target.set_rpy(ro, pi, ya)
p = arm.base.get_pos()
q = arm.base.get_rot()
P_b_w = Vector(p.x, p.y, p.z)
R_b_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
T_b_w = Frame(R_b_w, P_b_w)
p = arm.target.get_pos()
q = arm.target.get_rot()
P_t_w = Vector(p.x, p.y, p.z)
R_t_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
T_t_w = Frame(R_t_w, P_t_w)
T_t_b = T_b_w.Inverse() * T_t_w
# P_t_b = T_t_b.p
# P_t_b_scaled = P_t_b / self.psm1_scale
# T_t_b.p = P_t_b_scaled
computed_q = compute_IK(T_t_b)
# print('SETTING JOINTS: ')
# print(computed_q)
arm.base.set_joint_pos('baselink-yawlink', computed_q[0])
arm.base.set_joint_pos('yawlink-pitchbacklink', computed_q[1])
arm.base.set_joint_pos('pitchendlink-maininsertionlink', computed_q[2])
arm.base.set_joint_pos('maininsertionlink-toolrolllink', computed_q[3])
arm.base.set_joint_pos('toolrolllink-toolpitchlink', computed_q[4])
arm.base.set_joint_pos('toolpitchlink-toolyawlink', -computed_q[5])
arm.base.set_joint_pos('toolyawlink-toolgripper1link', gr)
arm.base.set_joint_pos('toolyawlink-toolgripper2link', gr)
for i in range(3):
if arm.sensor.is_triggered(i) and gr <= 0.2:
sensed_obj = arm.sensor.get_sensed_object(i)
if sensed_obj == 'Needle':
if not arm.grasped[i]:
arm.actuators[i].actuate(sensed_obj)
arm.grasped[i] = True
print('Grasping Sensed Object Names', sensed_obj)
else:
if gr > 0.2:
arm.actuators[i].deactuate()
arm.grasped[i] = False
def __init__(self, name):
pose_topic_name = name + 'pose'
twist_topic_name = name + 'twist'
button_topic_name = name + 'button'
force_topic_name = name + 'force_feedback'
self._active = False
self._scale = 0.0002
self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.twist = PyKDL.Twist()
# This offset is to align the pitch with the view frame
R_off = Rotation.RPY(0.0, 0, 0)
self._T_baseoffset = Frame(R_off, Vector(0, 0, 0))
self._T_baseoffset_inverse = self._T_baseoffset.Inverse()
self._T_tipoffset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.clutch_button_pressed = False # Used as Position Engage Clutch
self.gripper_button_pressed = False # Used as Gripper Open Close Binary Angle
self._force = DeviceFeedback()
self._force.force.x = 0
self._force.force.y = 0
self._force.force.z = 0
self._force.position.x = 0
self._force.position.y = 0
self._force.position.z = 0
self._pose_sub = rospy.Subscriber(pose_topic_name,
PoseStamped,
self.pose_cb,
queue_size=1)
self._twist_sub = rospy.Subscriber(twist_topic_name,
Twist,
self.twist_cb,
queue_size=1)
###### Commented
self._button_sub = rospy.Subscriber(button_topic_name,
DeviceButtonEvent,
self.buttons_cb,
queue_size=1)
self._force_pub = rospy.Publisher(force_topic_name,
DeviceFeedback,
queue_size=1)
self.switch_psm = False
self._button_msg_time = rospy.Time.now()
self._switch_psm_duration = rospy.Duration(0.5)
print('Creating Geomagic Device Named: ', name, ' From ROS Topics')
self._msg_counter = 0
def attach_needle(needle, link):
error = 1000
if link is None:
print('Not a valid link, returning')
return
while error > 0.1 and not rospy.is_shutdown():
P_tINw = Vector(link.get_pos().x, link.get_pos().y, link.get_pos().z)
# R_tINw = Rotation.Quaternion(tool_yaw_link.get_rot().x,
# tool_yaw_link.get_rot().y,
# tool_yaw_link.get_rot().x,
# tool_yaw_link.get_rot().w)
R_tINw = Rotation.RPY(link.get_rpy()[0],
link.get_rpy()[1],
link.get_rpy()[2])
# we need to move the needle based on the Pose of the toolyawlink.
# The yawlink's negative Y direction faces the graspp
# position. Therefore, lets add a small offset to the P of yawlink.
y_offset = Vector(0, -0.09, 0)
P_nINw = P_tINw + R_tINw * y_offset
# If you want to rotate the needle to a certain relative orientation
# add another Rotation and multiply on the R.H.S of R_tINw in the
# Equation below.
R_nINw = R_tINw * Rotation.RPY(0, 0, 3.14)
needle.set_pos(P_nINw[0], P_nINw[1], P_nINw[2])
needle.set_rot(R_nINw.GetQuaternion())
time.sleep(0.001)
P_tINw = Vector(link.get_pos().x, link.get_pos().y, link.get_pos().z)
P_nINw = Vector(needle.get_pos().x,
needle.get_pos().y,
needle.get_pos().z)
error = (P_tINw - P_nINw).Norm()
print error
# Wait for the needle to get there
time.sleep(3)
# You should see the needle in the center of the two fingers.
# If the gripper is not already closed, you shall have to manually
# close it to grasp the needle. You should probably automate this in the testIK script.
# Don't forget to release the pose command from the needle. We can
# do so by calling:
needle.set_force(0, 0, 0)
needle.set_torque(0, 0, 0)
def build_chain(self):
self.chain = Chain()
for e in self._config:
v = Vector(e["xyzrpy"][0], e["xyzrpy"][1], e["xyzrpy"][2])
r = Rotation.RPY(e["xyzrpy"][3], e["xyzrpy"][4], e["xyzrpy"][5])
j = Joint(e["name"], Joint.None)
f = Frame(r, v)
if e["type"] == "rotx":
axis = Vector(1.0, 0.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "roty":
axis = Vector(0.0, 1.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "rotz":
axis = Vector(0.0, 0.0, 1.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "transx":
axis = Vector(1.0, 0.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
elif e["type"] == "transy":
axis = Vector(0.0, 1.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
elif e["type"] == "transz":
axis = Vector(0.0, 0.0, 1.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
self.chain.addSegment(
Segment(e["name"] + "seg", j, f, RigidBodyInertia()))
self.fksolverpos = ChainFkSolverPos_recursive(self.chain)
def diff_calc_func(start_mat, end_mat, corrected_avg_interval):
temp = start_mat.M.Inverse() * end_mat.M
ang, temp_axis = Rotation.GetRotAngle(temp)
o = start_mat.M * temp_axis
p_start_vec = start_mat.p
p_end_vec = end_mat.p
# print "p vectors ", p_end_vec, p_start_vec
# Finding difference in position values between start and end
delta_x = p_end_vec[0] - p_start_vec[0]
delta_y = p_end_vec[1] - p_start_vec[1]
delta_z = p_end_vec[2] - p_start_vec[2]
# Calculation of differentiated values such as velocity/acceleration
twist_val = geometry_msgs.msg.Vector3()
twist_val.x = delta_x / corrected_avg_interval.to_sec()
twist_val.y = delta_y / corrected_avg_interval.to_sec()
twist_val.z = delta_z / corrected_avg_interval.to_sec()
# twist_rot = geometry_msgs.msg.Vector3()
# Calculation of angular velocity/acceleration
twist_rot = o * (ang / corrected_avg_interval.to_sec())
# print "accel is ", twist_vel
# print "accel rot is ", twist_rot[2]
return twist_val, twist_rot
def pose_cb(data):
global trans
p = data.pose
trans = Frame(
Rotation.Quaternion(p.orientation.x, p.orientation.y, p.orientation.z,
p.orientation.w),
Vector(p.position.x, p.position.y, p.position.z))
def trigger(self):
sample = self.gyro.get_sample()
gs = Gyro()
gs.header.frame_id = self.frame_id
gs.header.stamp = rospy.Time.now()
gs.calibration_offset.x = 0.0
gs.calibration_offset.y = 0.0
gs.calibration_offset.z = self.offset
gs.angular_velocity.x = 0.0
gs.angular_velocity.y = 0.0
gs.angular_velocity.z = (sample - self.offset) * math.pi / 180.0
gs.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
self.pub.publish(gs)
imu = Imu()
imu.header.frame_id = self.frame_id
imu.header.stamp = rospy.Time.now()
imu.angular_velocity.x = 0.0
imu.angular_velocity.y = 0.0
imu.angular_velocity.z = (sample - self.offset) * math.pi / 180.0
imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1]
self.orientation += imu.angular_velocity.z * (imu.header.stamp -
self.prev_time).to_sec()
self.prev_time = imu.header.stamp
(imu.orientation.x, imu.orientation.y, imu.orientation.z,
imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion()
self.pub2.publish(imu)
def get_initial_position(self, robot):
if robot.simulation:
robot_to_gob = (0.04, 0.08)
else:
robot_to_gob = robot.actuators.PUMPS.get(self.pump_id).get("pos")
gob_to_robot = TransformStamped()
gob_to_robot.transform.translation.x = -robot_to_gob[0]
gob_to_robot.transform.translation.y = -robot_to_gob[1]
goal_pose = TransformStamped()
goal_pose.transform.translation.x = self.position[0]
goal_pose.transform.translation.y = self.position[1]
robot_pose = PoseStamped()
robot_pose.pose.position.x = robot.get_position()[0]
robot_pose.pose.position.y = robot.get_position()[1]
angle = self.get_initial_orientation(robot)
rot = Rotation.RotZ(angle)
q = rot.GetQuaternion()
goal_pose.transform.rotation.x = q[0]
goal_pose.transform.rotation.y = q[1]
goal_pose.transform.rotation.z = q[2]
goal_pose.transform.rotation.w = q[3]
gob_to_robot_kdl = transform_to_kdl(gob_to_robot)
robot_goal_pose_kdl = do_transform_frame(gob_to_robot_kdl, goal_pose)
return (robot_goal_pose_kdl.p.x(), robot_goal_pose_kdl.p.y())
def update_arm_pose(self):
gui = self.GUI
gui.App.update()
T_t_b = Frame(Rotation.RPY(gui.ro, gui.pi, gui.ya),
Vector(gui.x, gui.y, gui.z))
self.arm.move_cp(T_t_b)
self.arm.set_jaw_angle(gui.gr)
self.arm.run_grasp_logic(gui.gr)
def run(self):
rosRate = Rate(30)
broadcaster = StaticTransformBroadcaster()
while not is_shutdown():
rot = Rotation(self._rotMatrixArray[0], self._rotMatrixArray[1],
self._rotMatrixArray[2], self._rotMatrixArray[3],
self._rotMatrixArray[4], self._rotMatrixArray[5],
self._rotMatrixArray[6], self._rotMatrixArray[7],
self._rotMatrixArray[8])
quat = rot.GetQuaternion()
staticTransform = self._setTransform(self._robotBaseFrame,
self._HDFrame, quat)
broadcaster.sendTransform(staticTransform)
rosRate.sleep()
def _update_base_pose(self):
if not self._base_pose_updated:
p = self.base.get_pos()
q = self.base.get_rot()
P_b_w = Vector(p.x, p.y, p.z)
R_b_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
self._T_b_w = Frame(R_b_w, P_b_w)
self._T_w_b = self._T_b_w.Inverse()
self._base_pose_updated = True
def pose_cb(self, msg):
self._pose = msg
self._frame.p = Vector(self._pose.pose.position.x,
self._pose.pose.position.y,
self._pose.pose.position.z)
self._frame.M = Rotation.Quaternion(self._pose.pose.orientation.x,
self._pose.pose.orientation.y,
self._pose.pose.orientation.z,
self._pose.pose.orientation.w)
def _update_camera_pose(self):
if self._pose_changed is True:
p = self.camera_handle.get_pos()
q = self.camera_handle.get_rot()
P_c_w = Vector(p.x, p.y, p.z)
R_c_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
self._T_c_w = Frame(R_c_w, P_c_w)
self._T_w_c = self._T_c_w.Inverse()
self._pose_changed = False
def convert_mat_to_frame(mat):
frame = Frame(Rotation.RPY(0, 0, 0), Vector(0, 0, 0))
for i in range(3):
for j in range(3):
frame[(i, j)] = mat[i, j]
for i in range(3):
frame.p[i] = mat[i, 3]
return frame
def pose_msg_to_kdl_frame(msg_pose):
pose = msg_pose.transform
f = Frame()
f.p[0] = pose.translation.x
f.p[1] = pose.translation.y
f.p[2] = pose.translation.z
f.M = Rotation.Quaternion(pose.rotation.x, pose.rotation.y,
pose.rotation.z, pose.rotation.w)
return f
def pose_msg_to_kdl_frame(msg_pose):
pose = msg_pose.pose
f = Frame()
f.p[0] = pose.position.x
f.p[1] = pose.position.y
f.p[2] = pose.position.z
f.M = Rotation.Quaternion(pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
return f
def hydra_msg_to_kdl_frame(msg_hydra):
pose = msg_hydra.paddles[0].transform
f = Frame()
f.p[0] = 10*pose.translation.x
f.p[1] = 10*pose.translation.y
f.p[2] = 10*pose.translation.z
f.M = Rotation.Quaternion(pose.rotation.x,
pose.rotation.y,
pose.rotation.z,
pose.rotation.w)
return f
def __init__(self, leader, psm):
self.counter = 0
self.leader = leader
self.psm = psm
self.init_xyz = Vector(0.0, 0.0, -1.0)
self.init_rpy = Rotation.RPY(3.14, 0.0, 1.57079)
self.cmd_xyz = self.init_xyz
self.cmd_rpy = self.init_rpy
self.T_IK = None
def to_kdl_frame(urdf_frame):
f = Frame()
if urdf_frame is not None:
if 'xyz' in urdf_frame.attrib:
xyz = [float(i) for i in urdf_frame.attrib['xyz'].split()]
for i in range(0, 3):
f.p[i] = xyz[i]
if 'rpy' in urdf_frame.attrib:
rpy = [float(i) for i in urdf_frame.attrib['rpy'].split()]
f.M = Rotation.RPY(rpy[0], rpy[1], rpy[2])
return f
def process_commands(self):
if self._active and self._obj_handle is not None:
if self._obj_handle.is_wd_expired():
self._obj_active = False
if not self._obj_active:
cur_pos = Vector(self._obj_handle.get_pos().x,
self._obj_handle.get_pos().y,
self._obj_handle.get_pos().z)
cur_rpy = Vector(self._obj_handle.get_rpy()[0],
self._obj_handle.get_rpy()[1],
self._obj_handle.get_rpy()[2])
if self._obj_handle.is_wd_expired:
self._obj_active = True
else:
cur_pos = self._cmd_pos
cur_rpy = self._cmd_rpy
rot = Rotation.RPY(0, 0, 0)
if self._ambf_cam_handle is not None:
cam_rpy = self._ambf_cam_handle.get_rpy()
rot = Rotation.RPY(cam_rpy[0], cam_rpy[1], cam_rpy[2])
self._cmd_pos = cur_pos + rot * (self._lin_vel * self._lin_scale)
self._cmd_rpy = cur_rpy + (self._ang_vel * self._ang_scale)
self._obj_handle.set_pos(self._cmd_pos[0], self._cmd_pos[1],
self._cmd_pos[2])
self._obj_handle.set_rpy(self._cmd_rpy[0], self._cmd_rpy[1],
self._cmd_rpy[2])
if self._msg_counter % 500 == 0:
pass
if self._msg_counter >= 1000:
self._msg_counter = 0
self._msg_counter = self._msg_counter + 1
def update_arm_pose(self):
twist = self.leader.measured_cv()
if not self.leader.gripper_button_pressed:
self.cmd_xyz = self.cmd_xyz + twist.vel * 0.00005
rot_offset_correction = Rotation.RPY(0.0, 0.0, 0.0)
self.cmd_rpy = self.leader.measured_cp().M * self.init_rpy
self.T_IK = Frame(self.cmd_rpy, self.cmd_xyz)
self.psm.move_cp(self.T_IK)
self.psm.set_jaw_angle(self.leader.get_jaw_angle())
self.psm.run_grasp_logic(self.leader.get_jaw_angle())
def move_cv(self, twist, dt):
if type(twist) in [np.array, np.ndarray]:
v = Vector(twist[0], twist[1], twist[2]) * dt
w = Vector(twist[3], twist[4], twist[5]) * dt
elif type(twist) is Twist:
v = twist.vel * dt
w = twist.rot * dt
else:
raise TypeError
T_c_w = self.get_T_c_w()
T_cmd = Frame(Rotation.RPY(w[0], w[1], w[2]), v)
self.move_cp(T_c_w * T_cmd)
pass
def pose_to_frame(pose):
# type: (object) -> Frame
if isinstance(pose, PoseStamped):
pose = pose.pose
assert isinstance(pose, Pose)
pos = Vector(pose.position.x, pose.position.y, pose.position.z)
rot = Rotation.Quaternion(pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
frame = Frame(rot, pos)
return frame
def update_arm_pose(self):
self.update_T_b_c()
twist = self.leader.measured_cv()
self.cmd_xyz = self.active_psm.T_t_b_home.p
if not self.leader.clutch_button_pressed:
delta_t = self._T_c_b.M * twist.vel * 0.002
self.cmd_xyz = self.cmd_xyz + delta_t
self.active_psm.T_t_b_home.p = self.cmd_xyz
self.cmd_rpy = self._T_c_b.M * self.leader.measured_cp().M * Rotation.RPY(np.pi, 0, np.pi / 2)
self.T_IK = Frame(self.cmd_rpy, self.cmd_xyz)
self.active_psm.move_cp(self.T_IK)
self.active_psm.set_jaw_angle(self.leader.get_jaw_angle())
self.active_psm.run_grasp_logic(self.leader.get_jaw_angle())
def __init__(self, arm_name):
self._arm_name = arm_name
mtml = mtm('MTML')
mtmr = mtm('MTMR')
self._base_offset = Frame()
self._tip_offset = Frame()
self._adjusted_pose = Frame()
self._adjusted_pose_pre = None
self._scale = 1000
if arm_name == 'MTMR':
self._base_offset = Frame(Rotation.RPY(3.0397, -1.0422, -1.477115),
Vector(-0.182, -0.016, -0.262))
self._tip_offset = Frame(Rotation.RPY(-1.57079, 0 , -1.57079),
Vector(0,0,0))
elif arm_name == 'MTML':
self._base_offset = Frame(Rotation.RPY(3.0397, -1.0422, -1.477115),
Vector(0.182, -0.016, -0.262))
self._tip_offset = Frame(Rotation.RPY(-1.57079, 0 , -1.57079),
Vector(0, 0, 0))
else:
assert (arm_name == 'MTML' and arm_name == 'MTMR')
mtml.home()
mtmr.home()
mtml.set_wrench_body_orientation_absolute(True)
mtmr.set_wrench_body_orientation_absolute(True)
mtml.set_wrench_body_force((0, 0, 0))
mtmr.set_wrench_body_force((0, 0, 0))
self._pose = PoseStamped()
self._frame = Frame()
self._pose_sub = rospy.Subscriber('/dvrk/' + arm_name + '/position_cartesian_current', PoseStamped, self.pose_cb)
