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)
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
br.sendTransform(T_HB.p, T_HB.M.GetQuaternion(), rospy.Time.now(),
'base', 'bax_head')
br.sendTransform(T_HB.p, T_HB.M.GetQuaternion(), rospy.Time.now(),
def main(args):
psm3 = AutonomyModule(activate_ar=args.activate_ar,
activate_debug_visuals=args.activate_debug_visuals)
centroid_module = utils.centroid_module()
model, labels_dict = create_model()
#Sleep until the subscribers are ready.
time.sleep(0.20)
sleep_time = 1.5
#Centroid coordinates
# centroids = np.array([[168,228]]).astype(np.int32)
#Fix landmarks
base_position = Vector(*[+0.04527744, -0.02624656, -0.02356771])
height_low = +0.06738468
height_medium = +0.06053191
height_high = height_low - 0.05
answer = raw_input(
"Did set velocity? have you check if the homography is up to date? if yes write 'Y' to continue, else don't move the robot. "
)
if answer != 'Y':
print("Exiting the program")
exit(0)
else:
time.sleep(2)
print("Move to base position")
psm3.move_psm3_to(base_position)
time.sleep(1)
print("Move arm 5 times")
try:
while not rospy.core.is_shutdown():
# for _ in range(1):
#Calculate centroids
final_frame, mask = utils.test_img(psm3.right_frame, model,
labels_dict)
centroids_coord = calculate_centroids(mask, psm3.multi_arr_pub,
centroid_module)
#Ask if you want to move to centroid
print("centroids", centroids_coord)
if centroids_coord.size > 0:
answer = 'Y' # raw_input("Do you want to move?")
if answer != 'Y':
print("Exiting the program")
exit(0)
else:
print("Move to base position")
psm3.move_psm3_to(base_position)
time.sleep(0.2)
# for i in range(centroids_coord.shape[0]):
# print("Movement {:d}".format(i))
pix = centroids_coord[0]
print(pix)
#AR animations
if args.activate_ar:
psm3.ar_animation(
base_position,
psm3.get_position_homography(
pix, height_medium))
time.sleep(1.0)
#Moving routine
psm3.move_using_homography(pix, height_high)
psm3.dvrk_controller.activate_ar = False
psm3.move_using_homography(pix, height_medium)
time.sleep(sleep_time - 0.6)
psm3.move_using_homography(pix, height_low)
time.sleep(sleep_time + 0.6)
psm3.move_using_homography(pix, height_medium)
time.sleep(sleep_time - 0.6)
psm3.move_using_homography(pix, height_low)
time.sleep(sleep_time + 0.6)
# psm3.move_using_homography(pix,height_medium); time.sleep(sleep_time-0.2);
# psm3.move_using_homography(pix,height_low);time.sleep(sleep_time+0.2);
#time.sleep(4.5)
psm3.move_using_homography(pix, height_high)
psm3.move_psm3_to(base_position)
# time.sleep(sleep_time)
else:
print("No pools of blood detected")
# try:
while not rospy.core.is_shutdown():
rospy.rostime.wallsleep(0.25)
except KeyboardInterrupt:
psm3.move_psm3_to(base_position)
print("Shutting down")
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]
if __name__ == '__main__':
rospy.init_node('baxter_optitrack_transformer')
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rate = rospy.Rate(50.0)
while not rospy.is_shutdown():
# Transformation -------------------------------------------------------------
#T_GB_p=Vector(0.0707,0.0187,-0.8593) # project
#T_GB_Q=Rotation.Quaternion(-0.004002,-0.004752,0.999994,-0.000251)
#T_GB=Frame(T_GB_Q,T_GB_p) [0.101118 <-0.573142, 0.049050, 0.811713>] (0.421538700623461, 0.5060896478392994, 0.5964256398717394, -0.45875358127230415)
T_GB_p = Vector(-0.0157712999511, -0.835218066682,
-0.0674771192051) #INKA
T_GB_Q = Rotation.Quaternion(-0.484148, -0.486831, -0.522866,
0.505181) # qz,qx,qy,qw INKA
#T_GB_p=Vector(0.0366584397092, -0.839661563598, -0.202769519381) #JA
#T_GB_Q=Rotation.Quaternion(0.421538700623461, 0.5060896478392994, 0.5964256398717394, -0.45875358127230415) # qz,qx,qy,qw JA
T_GB = Frame(T_GB_Q, T_GB_p)
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)
Rotx_p = Vector(0, 0, 0)
Rotx_Q = Rotation.Quaternion(0.70682518, 0, 0, 0.70682518) # x za 90
Rotx = Frame(Rotx_Q, Rotx_p)
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
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)
corrected_average_interval = end_time - start_time
# Get translation and quaternion of tf frame
start_pos, start_quat = listener.lookupTransform(
reference_frame, tracking_frame, start_time)
# print "start quat is ", start_quat
# Get translation and quaternion of tf frame
end_pos, end_quat = listener.lookupTransform(
reference_frame, tracking_frame, end_time)
# Create 4x4 homogeneous transformation matrix using position and quaternion values
start_mat = Frame(
Rotation.Quaternion(start_quat[0], start_quat[1],
start_quat[2], start_quat[3]),
Vector(start_pos[0], start_pos[1], start_pos[2]))
end_mat = Frame(
Rotation.Quaternion(end_quat[0], end_quat[1], end_quat[2],
end_quat[3]),
Vector(end_pos[0], end_pos[1], end_pos[2]))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
# Call the velocity function
twist_vel, twist_rot = vel_calc_func(start_mat, end_mat,
corrected_average_interval)
# print twist_vel, twist_rot
# Publish the output as wrench topic so that it can be visualized
pitch = 0
yaw = 0
# Initialize ROS
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)
Wrench,
wrench_cb,
queue_size=10)
p_sub = rospy.Subscriber('/dvrk/MTMR/position_cartesian_current',
PoseStamped,
pose_cb,
queue_size=10)
pub = rospy.Publisher('/wrench_visualization/wrench',
WrenchStamped,
queue_size=10)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
if wrench is not None:
force = Vector(wrench.wrench.force.x, wrench.wrench.force.y,
wrench.wrench.force.z)
body_force = trans.M.Inverse() * force
body_wrench = WrenchStamped()
body_wrench.header.frame_id = 'MTMR_wrist_roll_link'
body_wrench.wrench.force.x = body_force[0]
body_wrench.wrench.force.y = body_force[1]
body_wrench.wrench.force.z = body_force[2]
pub.publish(body_wrench)
rate.sleep()
w_sub.unregister()
p_sub.unregister()
pub.unregister()
def __init__(self, name):
pose_sub_topic_name = name + 'position_cartesian_current'
twist_topic_name = name + 'twist_body_current'
joint_state_sub_topic_name = name + 'state_joint_current'
gripper_topic_name = name + 'state_gripper_current'
clutch_topic_name = '/dvrk/footpedals/clutch'
coag_topic_name = '/dvrk/footpedals/coag'
pose_pub_topic_name = name + 'set_position_cartesian'
wrench_pub_topic_name = name + 'set_wrench_body'
effort_pub_topic_name = name + 'set_effort_joint'
self.cur_pos_msg = None
self.pre_coag_pose_msg = None
self._active = False
self._scale = 1.0
self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.twist = PyKDL.Twist()
R_off = Rotation.RPY(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.coag_button_pressed = False # Used as Position Engage Coag
self.gripper_angle = 0
self._pose_sub = rospy.Subscriber(pose_sub_topic_name,
PoseStamped,
self.pose_cb,
queue_size=1)
self._state_sub = rospy.Subscriber(joint_state_sub_topic_name,
JointState,
self.state_cb,
queue_size=1)
self._gripper_sub = rospy.Subscriber(gripper_topic_name,
JointState,
self.gripper_cb,
queue_size=1)
self._twist_sub = rospy.Subscriber(twist_topic_name,
TwistStamped,
self.twist_cb,
queue_size=1)
self._clutch_button_sub = rospy.Subscriber(clutch_topic_name,
Joy,
self.clutch_buttons_cb,
queue_size=1)
self._coag_button_sub = rospy.Subscriber(coag_topic_name,
Joy,
self.coag_buttons_cb,
queue_size=1)
self._pos_pub = rospy.Publisher(pose_pub_topic_name,
Pose,
queue_size=1)
self._wrench_pub = rospy.Publisher(wrench_pub_topic_name,
Wrench,
queue_size=1)
self._effort_pub = rospy.Publisher(effort_pub_topic_name,
JointState,
queue_size=1)
self.switch_psm = False
self._button_msg_time = rospy.Time.now()
self._switch_psm_duration = rospy.Duration(0.5)
self._jp = []
self._jv = []
self._jf = []
print('Creating MTM Device Named: ', name, ' From ROS Topics')
self._msg_counter = 0
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)
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)
def load_joint_data(self, ambf_config, urdf_joint):
joint_yaml_name = self.add_joint_prefix_str(urdf_joint.attrib['name'])
if urdf_joint.attrib['type'] in ['revolute', 'continuous', 'prismatic', 'fixed']:
joint = JointTemplate()
joint_data = joint.ambf_data
parent_body = self._bodies_map[urdf_joint.find('parent').attrib['link']]
child_body = self._bodies_map[urdf_joint.find('child').attrib['link']]
joint_data['name'] = urdf_joint.attrib['name']
joint_data['type'] = urdf_joint.attrib['type']
joint_data['parent'] = self.add_body_prefix_str(urdf_joint.find('parent').attrib['link'])
joint_data['child'] = self.add_body_prefix_str(urdf_joint.find('child').attrib['link'])
joint.origin = to_kdl_frame(urdf_joint.find('origin'))
if urdf_joint.attrib['type'] == 'fixed':
# If the joint is fixed, urdfs joint axis is ignore, we set it to universal nz
joint.axis = Vector(0, 0, 1)
else:
joint.axis = to_kdl_vec(urdf_joint.find('axis'))
parent_temp_frame = parent_body.visual_offset.Inverse() * joint.origin
parent_pivot = parent_temp_frame.p
parent_axis = parent_temp_frame.M * joint.axis
parent_pivot_data = joint_data["parent pivot"]
parent_axis_data = joint_data["parent axis"]
assign_xyz(parent_pivot_data, parent_pivot)
assign_xyz(parent_axis_data, parent_axis)
child_temp_frame = child_body.visual_offset
inv_child_temp_frame = child_temp_frame.Inverse()
child_pivot = inv_child_temp_frame.p
child_axis = inv_child_temp_frame.M * joint.axis
# The use of pivot and axis does not fully define the connection and relative transform between two bodies
# it is very likely that we need an additional offset of the child body as in most of the cases of URDF's
# For this purpose, we calculate the offset as follows
r_c_p_ambf = rot_matrix_from_vecs(child_axis, parent_axis)
r_c_p_urdf = parent_body.visual_offset.M.Inverse() * joint.origin.M * child_body.visual_offset.M
r_angular_offset = r_c_p_ambf.Inverse() * r_c_p_urdf
offset_axis_angle = r_angular_offset.GetRotAngle()
# print(axis_angle[0]),
# print(round(axis_angle[1][0], 1), round(axis_angle[1][1], 1), round(axis_angle[1][2], 1))
if abs(offset_axis_angle[0]) > 0.01:
# print '*****************************'
# print joint_data['name']
# print 'Joint Axis, '
# print '\t', joint.axis
# print 'Offset Axis'
# print '\t', offset_axis_angle[1]
offset_angle = round(offset_axis_angle[0], 3)
offset_axis = offset_axis_angle[1]
# print 'Offset Angle: \t', offset_angle
if abs(1.0 - dot(child_axis, offset_axis_angle[1])) < 0.1:
joint_data['offset'] = offset_angle
# print ': SAME DIRECTION'
elif abs(1.0 + dot(child_axis, offset_axis_angle[1])) < 0.1:
joint_data['offset'] = -offset_angle
# print ': OPPOSITE DIRECTION'
else:
print ('ERROR ', joint_data['name'], 'type', urdf_joint.attrib['type'], ': SHOULD\'NT GET HERE')
# print ('Offset Angle: ', offset_angle)
# print ('Offset Axis: ', offset_axis)
# print ('Joint Axis: ', joint.axis)
# r_c_p_ambf = self.round_mat(r_c_p_ambf)
# r_c_p_urdf = self.round_mat(r_c_p_urdf)
# r_angular_offset = self.round_mat(r_angular_offset)
#
# print ('R URDF: ')
# print (r_c_p_urdf)
# print ('R AMBF')
# print (r_c_p_ambf)
# print ('R_DIFF')
# print (r_angular_offset)
# print ('-----------------------------')
child_pivot_data = joint_data["child pivot"]
child_axis_data = joint_data["child axis"]
# There is a bug in bullet discussed here:
# https: // github.com / bulletphysics / bullet3 / issues / 2031
# As a work around, we want to tweak the axis or body masses just a bit
# It's better to tweak masses than axes
if parent_body.ambf_data['mass'] > 0.0:
factA = 1.0 / parent_body.ambf_data['mass']
if child_body.ambf_data['mass'] > 0.0:
factB = 1.0 / child_body.ambf_data['mass']
weighted_axis = factA * parent_axis + factB * child_axis
if weighted_axis.Norm() < 0.001:
print("WARNING: ", "Weighted Axis for joint \"%s\" is zero, to avoid breaking Bullet, "
"increasing the mass of parent body \"%s\" and decreasing the mass"
" of child body \"%s\" by 1%%"
% (joint.ambf_data['name'], parent_body.ambf_data['name'], child_body.ambf_data['name']))
parent_body.ambf_data['mass'] = parent_body.ambf_data['mass'] * 1.01
child_body.ambf_data['mass'] = child_body.ambf_data['mass'] * 0.99
assign_xyz(child_pivot_data, child_pivot)
assign_xyz(child_axis_data, child_axis)
if urdf_joint.attrib['type'] == 'continuous':
del joint_data['joint limits']['low']
del joint_data['joint limits']['high']
else:
urdf_joint_limit = urdf_joint.find('limit')
if urdf_joint_limit is not None:
joint_limit_data = joint_data["joint limits"]
joint_limit_data['low'] = round(float(urdf_joint_limit.attrib['lower']), 3)
joint_limit_data['high'] = round(float(urdf_joint_limit.attrib['upper']), 3)
ambf_config[joint_yaml_name] = joint_data
self._joint_names_list.append(joint_yaml_name)
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)
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)
def _get_frame(self):
return Frame(Vector(*self.pos)) * self.rot_frame
(lin_twist_start,
ang_twist_start) = listener.lookupTwist(reference_frame,
tracking_frame,
start_time,
rospy.Duration(0.1))
(lin_twist_end,
ang_twist_end) = listener.lookupTwist(reference_frame,
tracking_frame, end_time,
rospy.Duration(0.1))
# print "start quat is ", start_quat
# Create matrices for calculation of velocity which can be given into the custom function
start_mat_vel = Frame(
Rotation.Quaternion(start_quat[0], start_quat[1],
start_quat[2], start_quat[3]),
Vector(start_pos[0], start_pos[1], start_pos[2]))
end_mat_vel = Frame(
Rotation.Quaternion(end_quat[0], end_quat[1], end_quat[2],
end_quat[3]),
Vector(end_pos[0], end_pos[1], end_pos[2]))
# Create matrices for calculation of acceleration which can be given into the custom function
start_mat_acc = Frame(
Rotation.Quaternion(start_quat[0], start_quat[1],
start_quat[2], start_quat[3]),
Vector(lin_twist_start[0], lin_twist_start[1],
lin_twist_start[2]))
end_mat_acc = Frame(
Rotation.Quaternion(end_quat[0], end_quat[1], end_quat[2],
end_quat[3]),
Vector(lin_twist_end[0], lin_twist_end[1], lin_twist_end[2]))
