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 _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 _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 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 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 convert_DH_to_ambf_joint(self, a, alpha, d, theta, joint_data):
rot_bINa = Rotation(math.cos(theta), - math.sin(theta) * math.cos(alpha), math.sin(theta) * math.sin(alpha),
math.sin(theta), math.cos(theta) * math.cos(alpha), - math.cos(theta) * math.sin(alpha),
0, math.sin(alpha), math.cos(alpha))
pos_bINa = Vector(a * math.cos(theta), a * math.sin(theta), d)
for i in range(0, 3):
pos_bINa[i] = round(pos_bINa[i], 3)
for j in range(0, 3):
rot_bINa[i, j] = round(rot_bINa[i, j], 3)
trans_bINa = Frame(rot_bINa, pos_bINa)
# The child pivot and axis defined the joint in child frame. So we set
# the parent pivot and axis to zero and default respectively and then
# invert the Trans of B in A to get Trans of A in B and set the child
# pivot and axis based on that
trans_aINb = trans_bINa.Inverse()
print('DH: a: %s alpha: %s d: %s theta: %s', a, alpha, d, theta)
print(rot_bINa)
print(pos_bINa)
print('------')
parent_pivot = {'x': 0, 'y': 0, 'z': 0}
parent_axis = {'x': 0, 'y': 0, 'z': 1}
child_pivot = {'x': round(trans_aINb.p[0], 3),
'y': round(trans_aINb.p[1], 3),
'z': round(trans_aINb.p[2], 3)}
child_axis = {'x': round(trans_aINb.M[0, 2], 3),
'y': round(trans_aINb.M[1, 2], 3),
'z': round(trans_aINb.M[2, 2], 3)}
print(parent_pivot)
print(parent_axis)
print(child_pivot)
print(child_axis)
print('---------')
joint_data['parent axis'] = parent_axis
joint_data['parent pivot'] = parent_pivot
joint_data['child pivot'] = child_pivot
joint_data['child axis'] = child_axis
def fk(self, chain_state, link_num=-2):
if link_num < 0:
link_num -= 1
link_num += 1
if link_num < 0:
link_num = self._M
pos_scaled = [
cur_pos * cur_gearing for cur_pos, cur_gearing in zip(
chain_state.actual.positions, self._gearing)
]
#pos_scaled = JntArray(pos_scaled)
jnt_states = JntArray(len(pos_scaled))
for i, s in enumerate(pos_scaled):
jnt_states[i] = s
self.build_chain()
F1 = Frame()
assert (0 == self.fksolverpos.JntToCart(jnt_states, F1, link_num))
out = []
for i in range(3):
line = []
for j in range(3):
line.append(F1.M[i, j])
line.append(F1.p[i])
out.append(line)
out.append([0, 0, 0, 1])
out = matrix(out)
return out
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 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 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 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 __init__(self):
rospy.init_node('dvrk_slicer')
self._mtml = DvrkArm('MTML')
self._mtmr = DvrkArm('MTMR')
self._mtmr_pos_pre = None
self._mtml_pos_pre = None
self._footpedals = DvrkFootPedals()
self._cam_transform = Frame()
self._probe_transform = Frame()
self._igtl_cam_trans = igtltransform()
self._igtl_cam_trans.name = 'CameraTransform'
self._igtl_cam_trans.transform.rotation.w = 1.0
self._igtl_probe_trans = igtltransform()
self._igtl_probe_trans.name = 'ProbeTransform'
self._igtl_probe_trans.transform.rotation.w = 1.0
self._igtl_fiducial_point = igtlpoint()
self._igtl_fiducial_point.name = 'ENTRY'
self._fiducial_offset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self._igtl_text = igtlstring()
self._igtl_cam_trans_pub = rospy.Publisher('/IGTL_TRANSFORM_OUT', igtltransform, queue_size=1)
self._igtl_probe_trans_pub = rospy.Publisher('/IGTL_TRANSFORM_OUT', igtltransform, queue_size=1)
self._igtl_fiducial_pub = rospy.Publisher('/IGTL_POINT_OUT', igtlpoint, queue_size=1)
self._igtl_status_pub = rospy.Publisher('/IGTL_TEXT_OUT', igtlstring, queue_size=1)
self._rate = rospy.Rate(120)
self._pub_msg_pairs = dict()
self._pub_msg_pairs[self._igtl_cam_trans_pub] = self._igtl_cam_trans
self._pub_msg_pairs[self._igtl_probe_trans_pub] = self._igtl_probe_trans
self._pub_msg_pairs[self._igtl_fiducial_pub] = self._igtl_fiducial_point
# self._pub_msg_pairs[self._igtl_status_pub] = self._igtl_text
self._fiducial_placement_modes = ['ENTRY', 'TARGET']
self._fiducial_placement_active_mode = 0
self._cam_init_pose_computed = False
self._r_init_pos = Vector()
self._l_init_pos = Vector()
self._init_vec = Vector()
def __init__(self):
# Get the ~private namespace parameters from command li
self.poseArray = PoseArray()
self.waypointArray = PoseArray()
self.filename = "/home/shart/marker_trajectories/hose/hose_waypoints_1.txt"
self.pathPub = rospy.Publisher("/path_publisher/path", Path)
self.posePub = rospy.Publisher("/path_publisher/poseArray", PoseArray)
self.waypointPub = rospy.Publisher("/path_publisher/waypointArray",
PoseArray)
self.num_points = 10
self.frameOffset = Frame()
def test_roundtrip(self):
c = Frame()
d = pm.fromMsg(pm.toMsg(c))
self.assertEqual(repr(c), repr(d))
d = pm.fromMatrix(pm.toMatrix(c))
self.assertEqual(repr(c), repr(d))
d = pm.fromTf(pm.toTf(c))
self.assertEqual(repr(c), repr(d))
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 __init__(self):
self.ambf_data = OrderedDict()
self.ambf_data['name'] = ""
self.ambf_data['mesh'] = ""
self.ambf_data['mass'] = 0.0
self.ambf_data['inertia'] = {'ix': 0.0, 'iy': 0.0, 'iz': 0.0}
self.ambf_data['scale'] = 1.0
self.ambf_data['location'] = {
'position': {
'x': 0,
'y': 0,
'z': 0
},
'orientation': {
'r': 0,
'p': 0,
'y': 0
}
}
self.ambf_data['inertial offset'] = {
'position': {
'x': 0,
'y': 0,
'z': 0
},
'orientation': {
'r': 0,
'p': 0,
'y': 0
}
}
self.ambf_data['color'] = 'random'
self.inertial_offset = Frame()
self.visual_offset = Frame()
self.collision_offset = Frame()
self.parent = None
self.children = []
# If the urdf link has not visual geometry, it means that its purely for offsets and hence
# mark it as kinematic
self.is_kinematic = False
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 compute_effector_position(self, msg):
positions = JntArray(3)
chain = Chain()
kdl_frame = Frame()
d = 0
theta = 0
order = [1, 2, 0]
for i in order:
joint = self.params[i]
a = joint["a"]
d = joint["d"]
alpha = joint["alpha"]
theta = joint["theta"]
frame = kdl_frame.DH(a, alpha, d, theta)
chain.addSegment(Segment(Joint(Joint.RotZ), frame))
positions[i] = msg.position[i]
fk_solver = ChainFkSolverPos_recursive(chain)
result = Frame()
fk_solver.JntToCart(positions, result)
kdl_pose = PoseStamped()
kdl_pose.header.frame_id = 'base_link'
kdl_pose.header.stamp = rospy.Time.now()
kdl_pose.pose.position.x = result.p[0]
kdl_pose.pose.position.y = result.p[1]
kdl_pose.pose.position.z = result.p[2]
quat = result.M.GetQuaternion()
kdl_pose.pose.orientation.x = quat[0]
kdl_pose.pose.orientation.y = quat[1]
kdl_pose.pose.orientation.z = quat[2]
kdl_pose.pose.orientation.w = quat[3]
return kdl_pose
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 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_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 update_arm_pose(self):
self.update_T_b_c()
if self.leader.coag_button_pressed or self.leader.clutch_button_pressed:
self.leader.optimize_wrist_platform()
else:
if self.leader.is_active():
self.leader.move_cp(self.leader.pre_coag_pose_msg)
twist = self.leader.measured_cv() * 0.035
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
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
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 update_visual_markers(self):
# Move the Target Position Based on the GUI
if self.arm.target_IK is not None:
gui = self.GUI
T_ik_w = self.arm.get_T_b_w() * Frame(
Rotation.RPY(gui.ro, gui.pi, gui.ya),
Vector(gui.x, gui.y, gui.z))
self.arm.target_IK.set_pos(T_ik_w.p[0], T_ik_w.p[1], T_ik_w.p[2])
self.arm.target_IK.set_rpy(T_ik_w.M.GetRPY()[0],
T_ik_w.M.GetRPY()[1],
T_ik_w.M.GetRPY()[2])
if self.arm.target_FK is not None:
ik_solution = self.arm.get_ik_solution()
ik_solution = np.append(ik_solution, 0)
T_t_b = convert_mat_to_frame(compute_FK(ik_solution))
T_t_w = self.arm.get_T_b_w() * T_t_b
self.arm.target_FK.set_pos(T_t_w.p[0], T_t_w.p[1], T_t_w.p[2])
self.arm.target_FK.set_rpy(T_t_w.M.GetRPY()[0],
T_t_w.M.GetRPY()[1],
T_t_w.M.GetRPY()[2])
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)
def __init__(self, client, name):
self.client = client
self.name = name
self.base = self.client.get_obj_handle(name + '/baselink')
self.target_IK = self.client.get_obj_handle(name + '_target_ik')
self.palm_joint_IK = self.client.get_obj_handle(name + '_palm_joint_ik')
self.target_FK = self.client.get_obj_handle(name + '_target_fk')
self.sensor = self.client.get_obj_handle(name + '/Sensor0')
self.actuators = []
self.actuators.append(self.client.get_obj_handle(name + '/Actuator0'))
time.sleep(0.5)
self.grasped = [False, False, False]
self.T_t_b_home = Frame(Rotation.RPY(3.14, 0.0, 1.57079), Vector(0.0, 0.0, -1.0))
# Transform of Base in World
self._T_b_w = None
# Transform of World in Base
self._T_w_b = None
self._base_pose_updated = False
self._num_joints = 6
self._ik_solution = np.zeros([self._num_joints])
self._last_jp = np.zeros([self._num_joints])