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 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 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 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 __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 __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 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)
def _getOrientation(self, data):
q = Quaternion()
if data:
global hydro
if hydro:
if 'theta' in data:
# Assume Yaw Orientation
orientation = Rotation.RotZ(data.get('theta'))
elif any(k in ['roll', 'pitch', 'yaw']
for k in data.iterkeys()):
# Assume RPY Orientation
orientation = Rotation.RPY(data.get('roll', 0),
data.get('pitch', 0),
data.get('yaw', 0))
elif any(k in ['w', 'x', 'y', 'z'] for k in data.iterkeys()):
orientation = Rotation.Quaternion(data.get('x', 0),
data.get('y', 0),
data.get('z', 0),
data.get('w', 0))
else:
orientation = Rotation()
orientation = orientation.GetQuaternion()
else:
if 'theta' in data:
# Assume Yaw Orientation
orientation = quaternion_from_euler(
0, 0, data.get('theta'))
elif any(k in ['roll', 'pitch', 'yaw']
for k in data.iterkeys()):
# Assume RPY Orientation
orientation = quaternion_from_euler(
data.get('roll', 0), data.get('pitch', 0),
data.get('yaw', 0))
elif any(k in ['w', 'x', 'y', 'z'] for k in data.iterkeys()):
orientation = (data.get('x', 0), data.get('y', 0),
data.get('z', 0), data.get('w', 0))
else:
orientation = (0, 0, 0, 0)
q.x, q.y, q.z, q.w = orientation
return q
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 set_gazebo_puck_state(position, velocity):
set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
puck_state = SetModelStateRequest()
puck_state.model_state.model_name = 'puck_gazebo'
puck_state.model_state.pose.position.x = position[0]
puck_state.model_state.pose.position.y = position[1]
puck_state.model_state.pose.position.z = 0.0
rot = Rotation.RPY(0, 0, position[2])
(puck_state.model_state.pose.orientation.x,
puck_state.model_state.pose.orientation.y,
puck_state.model_state.pose.orientation.z,
puck_state.model_state.pose.orientation.w) = rot.GetQuaternion()
puck_state.model_state.twist.linear.x = velocity[0]
puck_state.model_state.twist.linear.y = velocity[1]
puck_state.model_state.twist.linear.z = 0.
puck_state.model_state.twist.angular.z = velocity[2]
puck_state.model_state.reference_frame = 'air_hockey_table::Table'
response = set_model_state(puck_state)
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])
def test():
rospy.init_node('test_mtm')
d = MTM('/dvrk/MTMR/')
d.set_base_frame(Frame(Rotation.RPY(np.pi / 2, 0, 0), Vector()))
# rot_offset = Rotation.RPY(np.pi, np.pi/2, 0).Inverse()
# tip_offset = Frame(rot_offset, Vector(0, 0, 0))
# d.set_tip_frame(tip_offset)
err_last = 0.0
while not rospy.is_shutdown():
if d.coag_button_pressed:
d.optimize_wrist_platform()
else:
if d.is_active():
d.move_cp(d.pre_coag_pose_msg)
# [r, p, y] = d.measured_cp().M.GetRPY()
# f = 180.0 / 3.1404
# r = round(r * f, 2)
# p = round(p * f, 2)
# y = round(y * f, 2)
# print('Roll: ', r, ', Pitch: ', p, ', Yaw: ', y)
time.sleep(0.05)
def compute_IK(T_7_0):
palm_length = 0.0091 # Fixed length from the palm joint to the pinch joint
pinch_length = 0.0102 # Fixed length from the pinch joint to the pinch tip
tool_rcm_offset = 0.0156 # Delta between tool tip and the Remote Center of Motion
# Pinch Joint
T_PinchJoint_7 = Frame(Rotation.RPY(0, 0, 0), pinch_length * Vector(0.0, 0.0, -1.0))
# Pinch Joint in Origin
T_PinchJoint_0 = T_7_0 * T_PinchJoint_7
# It appears from the geometry of the robot, that the palm joint is always in the ZY
# plane of the end effector frame (7th Frame)
# This is the logic that should give us the direction of the palm link and then
# we know the length of the palm link so we can keep going back to find the shaftTip (PalmJoint)
# position
# Convert the vector from base to pinch joint in the pinch joint frame
# print("P_PinchJoint_0: ", round_vec(T_PinchJoint_0.p))
P_PinchJoint_local = T_PinchJoint_0.M.Inverse() * T_PinchJoint_0.p
# print("P_PinchJoint_local: ", round_vec(P_PinchJoint_local))
# Now we can trim the value along the x axis to get a projection along the YZ plane as mentioned above
N_PalmJoint_PinchJoint = P_PinchJoint_local
N_PalmJoint_PinchJoint[0] = 0
N_PalmJoint_PinchJoint.Normalize()
# We can check the angle to see if things make sense
angle = get_angle(N_PalmJoint_PinchJoint, Vector(0, 0, -1))
# print("Palm Link Angle in Pinch YZ Plane: ", angle)
# If the angle between the two vectors is > 90 Degree, we should move in the opposite direction
if angle > np.pi/2:
N_PalmJoint_PinchJoint = -N_PalmJoint_PinchJoint
# Add another frame to account for Palm link length
# print("N_PalmJoint_PinchJoint: ", round_vec(N_PalmJoint_PinchJoint))
T_PalmJoint_PinchJoint = Frame(Rotation.RPY(0, 0, 0), N_PalmJoint_PinchJoint * palm_length)
# print("P_PalmJoint_PinchJoint: ", round_vec(T_PalmJoint_PinchJoint.p))
# Get the shaft tip or the Palm's Joint position
T_PalmJoint_0 = T_7_0 * T_PinchJoint_7 * T_PalmJoint_PinchJoint
# print("P_PalmJoint_0: ", round_vec(T_PalmJoint_0.p))
# print("P_PinchJoint_0: ", round_vec(T_PinchJoint_0.p))
# Now this should be the position of the point along the RC
# print("Point Along the SHAFT: ", T_PalmJoint_0.p)
# Calculate insertion_depth to check if the tool is past the RCM
insertion_depth = T_PalmJoint_0.p.Norm()
if insertion_depth <= tool_rcm_offset:
sign = 1
elif insertion_depth > tool_rcm_offset:
sign = -1
# Now having the end point of the shaft or the PalmJoint, we can calculate some
# angles as follows
# xz_diagonal = math.sqrt(T_PalmJoint_0.p[0] ** 2 + T_PalmJoint_0.p[2] ** 2)
# # print ('XZ Diagonal: ', xz_diagonal)
# j1 = np.sign(T_PalmJoint_0.p[0]) * math.acos(-T_PalmJoint_0.p[2] / xz_diagonal)
j1 = math.atan2(T_PalmJoint_0.p[0], sign * T_PalmJoint_0.p[2])
# yz_diagonal = math.sqrt(T_PalmJoint_0.p[1] ** 2 + T_PalmJoint_0.p[2] ** 2)
# # print('YZ Diagonal: ', yz_diagonal)
# j2 = np.sign(T_PalmJoint_0.p[0]) * math.acos(-T_PalmJoint_0.p[2] / yz_diagonal)
j2 = -math.atan2(T_PalmJoint_0.p[1], sign * T_PalmJoint_0.p[2])
j3 = insertion_depth + tool_rcm_offset
# Calculate j4
# This is an important case and has to be dealt carefully. Based on some inspection, we can find that
# we need to construct a plane based on the vectors Rx_7_0 and D_PinchJoint_PalmJoint_0 since these are
# the only two vectors that are orthogonal at all configurations of the EE.
cross_palmlink_x7_0 = T_7_0.M.UnitX() * (T_PinchJoint_0.p - T_PalmJoint_0.p)
# To get j4, compare the above vector with Y axes of T_3_0
T_3_0 = convert_mat_to_frame(compute_FK([j1, j2, j3]))
j4 = get_angle(cross_palmlink_x7_0, T_3_0.M.UnitY(), up_vector=-T_3_0.M.UnitZ())
# Calculate j5
# This should be simple, just compute the angle between Rz_4_0 and D_PinchJoint_PalmJoint_0
T_4_0 = convert_mat_to_frame(compute_FK([j1, j2, j3, j4]))
j5 = get_angle(T_PinchJoint_0.p - T_PalmJoint_0.p, T_4_0.M.UnitZ(), up_vector=-T_4_0.M.UnitY())
# Calculate j6
# This too should be simple, compute the angle between the Rz_7_0 and Rx_5_0.
T_5_0 = convert_mat_to_frame(compute_FK([j1, j2, j3, j4, j5]))
j6 = get_angle(T_7_0.M.UnitZ(), T_5_0.M.UnitX(), up_vector=-T_5_0.M.UnitY())
str = '\n**********************************'*3
# print(str)
# print("Joint 1: ", round(j1, 3))
# print("Joint 2: ", round(j2, 3))
# print("Joint 3: ", round(j3, 3))
# print("Joint 4: ", round(j4, 3))
# print("Joint 5: ", round(j5, 3))
# print("Joint 6: ", round(j6, 3))
T_7_0_req = convert_frame_to_mat(T_7_0)
T_7_0_req = round_transform(T_7_0_req, 3)
# print('Requested Pose: \n', T_7_0_req)
T_7_0_computed = compute_FK([j1, j2, j3, j4, j5, j6, 0])
round_transform(T_7_0_computed, 3)
# print('Computed Pose: \n', T_7_0_computed)
return [j1, j2, j3, j4, j5, j6]
def compute_ang_error(set_point, cur_pos):
sp = Rotation.RPY(set_point[0], set_point[1], set_point[2])
dr = cur_pos.Inverse() * sp
ang, axis = dr.GetRotAngle()
dr = cur_pos * axis * ang
return dr
c.connect()
cam = Camera(c, 'CameraFrame')
time.sleep(0.5)
controllers = []
psm_arms = []
if parsed_args.run_psm_one is True:
# Initial Target Offset for PSM1
# init_xyz = [0.1, -0.85, -0.15]
arm_name = 'psm1'
print('LOADING CONTROLLER FOR ', arm_name)
psm = PSM(c, arm_name)
if psm.is_present():
T_psmtip_c = Frame(Rotation.RPY(3.14, 0.0, -1.57079), Vector(-0.2, 0.0, -1.0))
T_psmtip_b = psm.get_T_w_b() * cam.get_T_c_w() * T_psmtip_c
psm.set_home_pose(T_psmtip_b)
psm_arms.append(psm)
if parsed_args.run_psm_two is True:
# Initial Target Offset for PSM1
# init_xyz = [0.1, -0.85, -0.15]
arm_name = 'psm2'
print('LOADING CONTROLLER FOR ', arm_name)
theta_base = -0.7
psm = PSM(c, arm_name)
if psm.is_present():
T_psmtip_c = Frame(Rotation.RPY(3.14, 0.0, -1.57079), Vector(0.2, 0.0, -1.0))
T_psmtip_b = psm.get_T_w_b() * cam.get_T_c_w() * T_psmtip_c
psm.set_home_pose(T_psmtip_b)
# \author <http://www.aimlab.wpi.edu>
# \author <*****@*****.**>
# \author Adnan Munawar
# \version 0.1
# */
# //==============================================================================
import rospy
from geometry_msgs.msg import PoseStamped, Quaternion
from std_msgs.msg import Empty
import numpy as np
from PyKDL import Rotation
import tf_conversions
import sys
r = Rotation()
r.RPY(0,0,0)
rPose = PoseStamped()
lPose = PoseStamped()
rPose.pose.position.z = -0.30
lPose.pose.position.z = -0.20
rPose.pose.orientation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(3.058663, -1.055021, -1.500306))
lPose.pose.orientation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(3.058663, -1.055021, -1.500306))
tc = 4.0
scale = 1/15.0
if len(sys.argv) > 1:
tc = float(sys.argv[1])
print 'Setting Time Constant to {}'.format(sys.argv[1])
if len(sys.argv) > 2:
scale = float(sys.argv[2])
def handle(req):
global pose_now
global pub
if req.mode == 'poly': #bardzo wygodne przelaczenie funkcji interpolacji
fun = polynomial
else:
fun = linear
if not req.t > 0.0:
print >> stderr, "Incorect time value: %s" % req.t
return "time wrong value"
msg = PoseStamped()
msg.header.frame_id = 'base_link'
t = 0
rate = rospy.Rate(20)
while t < req.t:
msg.header.stamp = rospy.get_rostime()
msg.pose.position.x = fun(t, pose_now[0], req.x, req.t)
msg.pose.position.y = fun(t, pose_now[1], req.y, req.t)
msg.pose.position.z = fun(t, pose_now[2], req.z, req.t)
roll = fun(t, pose_now[3], req.roll, req.t)
pitch = fun(t, pose_now[4], req.pitch, req.t)
yaw = fun(t, pose_now[5], req.yaw, req.t)
rot = Rotation.RPY(roll, pitch, yaw)
quat = rot.GetQuaternion()
msg.pose.orientation.x = quat[0]
msg.pose.orientation.y = quat[1]
msg.pose.orientation.z = quat[2]
msg.pose.orientation.w = quat[3]
pub.publish(msg)
if req.t - t < 0.05:
t = req.t
else:
t += 0.05
rate.sleep()
msg.header.stamp = rospy.get_rostime()
msg.pose.position.x = fun(t, pose_now[0], req.x, req.t)
msg.pose.position.y = fun(t, pose_now[1], req.y, req.t)
msg.pose.position.z = fun(t, pose_now[2], req.z, req.t)
roll = fun(t, pose_now[3], req.roll, req.t)
pitch = fun(t, pose_now[4], req.pitch, req.t)
yaw = fun(t, pose_now[5], req.yaw, req.t)
rot = Rotation.RPY(roll, pitch, yaw)
quat = rot.GetQuaternion()
msg.pose.orientation.x = quat[0]
msg.pose.orientation.y = quat[1]
msg.pose.orientation.z = quat[2]
msg.pose.orientation.w = quat[3]
pub.publish(msg)
pose_now[0] = msg.pose.position.x
pose_now[1] = msg.pose.position.y
pose_now[2] = msg.pose.position.z
pose_now[3] = roll
pose_now[4] = pitch
pose_now[5] = yaw
return "Done"
elif parsed_args.run_psm_two in ['False', 'false', '0']:
parsed_args.run_psm_two = False
if parsed_args.run_psm_three in ['True', 'true', '1']:
parsed_args.run_psm_three = True
elif parsed_args.run_psm_three in ['False', 'false', '0']:
parsed_args.run_psm_three = False
c = Client()
c.connect()
cam_frame = c.get_obj_handle('CameraFrame')
time.sleep(0.5)
P_c_w = cam_frame.get_pos()
R_c_w = cam_frame.get_rpy()
T_c_w = Frame(Rotation.RPY(R_c_w[0], R_c_w[1], R_c_w[2]), Vector(P_c_w.x, P_c_w.y, P_c_w.z))
print(T_c_w)
controllers = []
psm_arms = []
if parsed_args.run_psm_one is True:
# Initial Target Offset for PSM1
# init_xyz = [0.1, -0.85, -0.15]
arm_name = 'psm1'
print('LOADING CONTROLLER FOR ', arm_name)
psm = PSM(c, arm_name)
if psm.is_present():
T_psmtip_c = Frame(Rotation.RPY(3.14, 0.0, -1.57079), Vector(-0.2, 0.0, -1.0))
T_psmtip_b = psm.get_T_w_b() * T_c_w * T_psmtip_c
psm.set_home_pose(T_psmtip_b)
rospy.init_node('ambf_control_test')
sub = rospy.Subscriber(object_name + 'State', ObjectState, ambf_cb, queue_size=1)
pub = rospy.Publisher(object_name + 'Command', ObjectCmd, queue_size=1)
rate = rospy.Rate(1000)
K_lin = 100.0
D_lin = 20.0
K_ang = 5
D_ang = 1
last_delta_pos = Vector(0, 0, 0)
delta_pos = Vector(0, 0, 0)
last_pos = Vector(0, 0, 0)
m_drot_prev = Rotation.RPY(0, 0, 0)
m_drot = Rotation.RPY(0, 0, 0)
last_rot = Rotation.RPY(0, 0, 0)
cmd_msg = ObjectCmd()
cmd_msg.enable_position_controller = False
dt = 0.001
cur_time = rospy.Time.now().to_sec()
torque = Vector(0, 0, 0)
last_torque = Vector(0, 0, 0)
d_torque = Vector(0, 0, 0)
# Main Loop
while not rospy.is_shutdown():
rospy.Time(0))
(trans_OA,
rot_OA) = listener.lookupTransform('/optitrack', '/bax_arm',
rospy.Time(0))
(trans_BG,
rot_BG) = listener.lookupTransform('/base', '/left_gripper_base',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
# Rotations
rot_OH = Rotation.Quaternion(*rot_OH)
rot_OA = Rotation.Quaternion(*rot_OA)
rot_BG = Rotation.Quaternion(*rot_BG)
rot_AG = Rotation.RPY(math.pi / 2, -math.pi, math.pi / 2)
# Creating Frames
T_OH = Frame(rot_OH, Vector(*trans_OH))
T_OA = Frame(rot_OA, Vector(*trans_OA))
T_BG = Frame(rot_BG, Vector(*trans_BG))
T_AG = Frame(rot_AG, Vector(0, 0, 0))
# Finding right transformation
T_HB = T_OH.Inverse() * T_OA * T_AG * T_BG.Inverse()
T_empty_p = Vector(0, 0, 0)
T_empty_Q = Rotation.Quaternion(0, 0, 0, 1)
T_empty = Frame(T_empty_Q, T_empty_p)
# Broadcast new transformations
def callback(data, args):
x = args[0]
y = args[1]
z = args[2]
roll = args[3]
pitch = args[4]
yaw = args[5]
rot1 = Rotation.RPY(roll[0], pitch[0], yaw[0])
rot2 = Rotation.RPY(roll[1], pitch[1], yaw[1])
rot3 = Rotation.RPY(roll[2], pitch[2], yaw[2])
vec1 = Vector(x[0], y[0], z[0])
vec2 = Vector(x[1], y[1], z[1])
vec3 = Vector(x[2], y[2], z[2])
rot1.DoRotZ(data.position[0])
rot2.DoRotZ(data.position[1])
rot3.DoRotZ(data.position[2])
T01 = Frame(rot1, vec1)
T12 = Frame(rot2, vec2)
T23 = Frame(rot3, vec3)
T02 = T01 * T12
T03 = T02 * T23
quat01 = T01.M.GetQuaternion()
quat02 = T02.M.GetQuaternion()
quat03 = T03.M.GetQuaternion()
if quat01[3] == 0 or quat02[3] == 0 or quat03[3] == 0:
rospy.logerr("quaternion calculation failed")
return
msg = PoseStamped()
msg.header.stamp = data.header.stamp
msg.header.frame_id = "/base_link"
msg.pose.position.x = T01.p.x()
msg.pose.position.y = T01.p.y()
msg.pose.position.z = T01.p.z()
msg.pose.orientation.x = quat01[0]
msg.pose.orientation.y = quat01[1]
msg.pose.orientation.z = quat01[2]
msg.pose.orientation.w = quat01[3]
pub1.publish(msg)
msg.pose.position.x = T02.p.x()
msg.pose.position.y = T02.p.y()
msg.pose.position.z = T02.p.z()
msg.pose.orientation.x = quat02[0]
msg.pose.orientation.y = quat02[1]
msg.pose.orientation.z = quat02[2]
msg.pose.orientation.w = quat02[3]
pub2.publish(msg)
msg.pose.position.x = T03.p.x()
msg.pose.position.y = T03.p.y()
msg.pose.position.z = T03.p.z()
msg.pose.orientation.x = quat03[0]
msg.pose.orientation.y = quat03[1]
msg.pose.orientation.z = quat03[2]
msg.pose.orientation.w = quat03[3]
pub3.publish(msg)
def build_frame(xyz=(0, 0, 0), rpy=(0, 0, 0)):
# type: (tuple, tuple) -> Frame
pos = Vector(xyz[0], xyz[1], xyz[2])
rot = Rotation.RPY(rpy[0], rpy[1], rpy[2])
return Frame(rot, pos)
elif parsed_args.run_psm_three in ['False', 'false', '0']:
parsed_args.run_psm_three = False
c = Client()
c.connect()
controllers = []
if parsed_args.run_psm_one is True:
# Initial Target Offset for PSM1
# init_xyz = [0.1, -0.85, -0.15]
arm_name = 'psm3'
print('LOADING CONTROLLER FOR ', arm_name)
leader = GeomagicDevice('/Geomagic/')
theta = -0.7
leader.set_base_frame(Frame(Rotation.RPY(theta, 0, 0), Vector(0, 0,
0)))
leader.set_tip_frame(Frame(Rotation.RPY(theta + 0.7, 0, 0), Vector()))
psm = PSM(c, arm_name)
controller = ControllerInterface(leader, psm)
controllers.append(controller)
if len(controllers) == 0:
print('No Valid PSM Arms Specified')
print('Exiting')
else:
while not rospy.is_shutdown():
for cont in controllers:
cont.run()
time.sleep(0.005)
elif parsed_args.run_psm_two in ['False', 'false', '0']:
parsed_args.run_psm_two = False
if parsed_args.run_psm_three in ['True', 'true', '1']:
parsed_args.run_psm_three = True
elif parsed_args.run_psm_three in ['False', 'false', '0']:
parsed_args.run_psm_three = False
c = Client()
c.connect()
cam_frame = c.get_obj_handle('CameraFrame')
time.sleep(0.5)
P_c_w = cam_frame.get_pos()
R_c_w = cam_frame.get_rpy()
T_c_w = Frame(Rotation.RPY(R_c_w[0], R_c_w[1], R_c_w[2]),
Vector(P_c_w.x, P_c_w.y, P_c_w.z))
print(T_c_w)
controllers = []
psm_arms = []
if parsed_args.run_psm_one is True:
# Initial Target Offset for PSM1
# init_xyz = [0.1, -0.85, -0.15]
arm_name = 'psm1'
print('LOADING CONTROLLER FOR ', arm_name)
psm = PSM(c, arm_name)
if psm.is_present():
T_psmtip_c = Frame(Rotation.RPY(3.14, 0.0, -1.57079),
Vector(-0.2, 0.0, -1.0))
