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