def forward_kinematics(self, angles, frame='jaw', ref='s'): if frame == 'jaw' and ref == 's': return mr.FKinSpace(self.M_s_jaw, self.S_s_jaw, angles) if frame == 'jaw' and ref == 'r': return mr.FKinSpace(self.M_r_jaw, self.S_r_jaw, angles) elif frame == 'end' and ref == 'r': return mr.FKinSpace(self.M_r_e, self.S_r_e, angles) elif frame == 'end' and ref == 's': return mr.FKinSpace(self.M_s_e, self.S_s_e, angles) else: raise Exception('Frame or reference is not right.')
def forward_kinematics(self, angles, frame='end'): if frame == 'camera': return mr.FKinSpace(self.M_camera, self.Slist, angles) elif frame == 'needle': return mr.FKinSpace(self.M_needle, self.Slist, angles) elif frame == 'gripper': return mr.FKinSpace(self.M_gripper, self.Slist, angles) elif frame == 'hot_air': return mr.FKinSpace(self.M_hot_air, self.Slist, angles) else: return mr.FKinSpace(self.M_end, self.Slist, angles)
def convert_FK(results): ''' convert joint angles to end effector position ''' H0 = 0.0723 H1 = 0.0333 L1 = 0.0132 L2 = 0.14207 L3 = 0.1585 L4 = 0.0566 H5 = 0.0751 # M: The home configuration of the end-effector M = np.array([[1, 0, 0, L1 + L2 + L3 + L4], [0, 1, 0, 0], [0, 0, 1, H0 + H1 - H5], [0, 0, 0, 1]]) Slist = np.array([[0, 0, 1, 0, 0, 0], [0, -1, 0, H0 + H1, 0, -L1], [0, 1, 0, -H0 - H1, 0, L1 + L2], [0, -1, 0, H0 + H1, 0, -L1 - L2 - L3], [0, 0, 1, 0, -L1 - L2 - L3 - L4, 0]]).T results_FK = results.copy() for i in range(results_FK.shape[0]): thetalist = np.hstack( (results[i, :3], (results[i, 2] - results[i, 1]), results[i, 4])) mat = mr.FKinSpace(M, Slist, thetalist) position = mat[:3, 3] results_FK[i, :3] = position return results_FK
def capture_joint_positions(self): self.joint_commands = [] for name in self.core.joint_names: self.joint_commands.append( self.core.joint_states.position[self.core.js_index_map[name]]) self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.joint_commands)
def set_single_joint_position(self, joint_name, position, moving_time=None, accel_time=None, blocking=True): self.set_trajectory_time(moving_time, accel_time) self.joint_positions[self.joint_indx_dict[joint_name]] = position single_command = SingleCommand(joint_name, position) self.pub_single_command.publish(single_command) if blocking: rospy.sleep(self.moving_time) self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.joint_positions)
def publish_positions(self, positions, moving_time=None, accel_time=None, blocking=True): self.set_trajectory_time(moving_time, accel_time) self.joint_commands = list(positions) joint_commands = JointGroupCommand(self.group_name, self.joint_commands) self.core.pub_group.publish(joint_commands) if blocking: rospy.sleep(self.moving_time) self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.joint_commands)
def get_ee_pose(self): joint_states = [ self.core.joint_states.position[self.core.js_index_map[name]] for name in self.group_info.joint_names ] T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, joint_states) return T_sb
def command_positions(self, positions, vel=1.0, accel=5.0, mode=0): self.joint_commands = list(positions) if self.core.mode != mode: self.core.robot_smart_mode_reset(mode) if (mode == 0): self.core.robot_move_joint(positions, vel, accel) elif (mode == 1): self.core.robot_move_servoj(positions) self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.joint_commands)
def get_transformations(self, robot_configuration): transformations = [] for screw_index in range(0, self.screws.shape[1]): transformation = mr.FKinSpace( self.end_effectors_at_zero_position[screw_index], self.screws[:, 0:screw_index + 1], robot_configuration.to_theta_list()[0:screw_index + 1]) transformations.append(transformation) return transformations
def set_single_joint_position(self, joint_name, position, moving_time=None, accel_time=None, blocking=True): if not self.check_single_joint_limit(joint_name, position): return False self.set_trajectory_time(moving_time, accel_time) self.joint_commands[self.core.js_index_map[joint_name]] = position single_command = JointSingleCommand(joint_name, position) self.core.pub_single.publish(single_command) if blocking: rospy.sleep(self.moving_time) self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.joint_commands) return True
def FindJointPositions(thetalist): M2 = np.array([[1,0,0,0], [0,1,0,0], [0,0,1,145], [0,0,0,1]]) M3 = np.array([[1,0,0,0], [0,1,0,0], [0,0,1,351], [0,0,0,1]]) M4 = np.array([[1,0,0,0], [0,1,0,0], [0,0,1,468], [0,0,0,1]]) M5 = np.array([[1,0,0,0], [0,1,0,0], [0,0,1,468], [0,0,0,1]]) M6 = np.array([[1,0,0,0], [0,1,0,117], [0,0,1,468], [0,0,0,1]]) M7 = np.array([[1,0,0,0], [0,1,0,117], [0,0,1,541], [0,0,0,1]]) Slist = Robot.Slist TJ1 = [[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]] TJ2 = mr.FKinSpace(M2,Slist[:2].T,thetalist[:2]) TJ3 = mr.FKinSpace(M3,Slist[:3].T,thetalist[:3]) TJ4 = mr.FKinSpace(M4,Slist[:4].T,thetalist[:4]) TJ5 = mr.FKinSpace(M5,Slist[:5].T,thetalist[:5]) TJ6 = mr.FKinSpace(M6,Slist[:6].T,thetalist[:6]) TEE = mr.FKinSpace(M7,Slist[:6].T,thetalist[:6]) J1 = Robot.Joint(TJ1[0][3], TJ1[1][3], TJ1[2][3]) J2 = Robot.Joint(TJ2[0][3], TJ2[1][3], TJ2[2][3]) J3 = Robot.Joint(TJ3[0][3], TJ3[1][3], TJ3[2][3]) J4 = Robot.Joint(TJ4[0][3], TJ4[1][3], TJ4[2][3]) J5 = Robot.Joint(TJ5[0][3], TJ5[1][3], TJ5[2][3]) J6 = Robot.Joint(TJ6[0][3], TJ6[1][3], TJ6[2][3]) EE = Robot.Joint(TEE[0][3], TEE[1][3], TEE[2][3]) Xlist = [J1.x,J2.x,J3.x,J4.x,J5.x,J6.x,EE.x] Ylist = [J1.y,J2.y,J3.y,J4.y,J5.y,J6.y,EE.y] Zlist = [J1.z,J2.z,J3.z,J4.z,J5.z,J6.z,EE.z] return Xlist, Ylist, Zlist
def FKallSpace(aList0,aList1,aList2,aList3,aList4,aList5): newPos0 = mrcl.FKinSpace(Mleg,Slist,aList0) newPos1 = mrcl.FKinSpace(Mleg,Slist,aList1) newPos2 = mrcl.FKinSpace(Mleg,Slist,aList2) newPos3 = mrcl.FKinSpace(Mleg,Slist,aList3) newPos4 = mrcl.FKinSpace(Mleg,Slist,aList4) newPos5 = mrcl.FKinSpace(Mleg,Slist,aList5) return newPos0,newPos1,newPos2,newPos3,newPos4,newPos5
def joy_control_cb(self, msg): with self.js_mutex: js_msg = self.joint_states.position with self.mutex: # Check if the gripper pwm or state should be changed if (self.resp.use_gripper == True): if (msg.gripper_pwm_cmd != 0): self.set_gripper_pwm(msg.gripper_pwm_cmd) if (msg.gripper_cmd != 0): self.set_gripper_cmd(msg.gripper_cmd, js_msg[-2]) if (msg.arm_speed_cmd != 0 or msg.arm_toggle_speed_cmd != 0): self.set_arm_speeds(msg.arm_speed_cmd, msg.arm_toggle_speed_cmd) # Check if the arm should go to a 'Home' or 'Sleep' pose if (msg.robot_pose != 0): self.set_robot_pose(msg.robot_pose) self.arm_state = ArmState.ROBOT_POSE_CONTROL self.last_active_arm_state = ArmState.ROBOT_POSE_CONTROL # Check if the end-effector should move horizontally and/or vertically elif (msg.ee_z_cmd != 0 or msg.ee_x_cmd != 0): self.set_velocity_ik(msg.ee_z_cmd, msg.ee_x_cmd) if (msg.ee_z_cmd == 0): if not (self.last_active_arm_state == ArmState.VELOCITY_IK and self.velocity_x_ik_only == True): self.ee_reference = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, js_msg[:self.num_joints]) self.velocity_x_ik_only = True else: self.velocity_x_ik_only = False self.arm_state = ArmState.VELOCITY_IK self.last_active_arm_state = ArmState.VELOCITY_IK # Check if indiviual joints should be rotated elif (msg.waist_cmd != 0 or msg.wrist_angle_cmd != 0 or msg.wrist_rotate_cmd != 0): self.set_single_joint(msg.waist_cmd, msg.wrist_angle_cmd, msg.wrist_rotate_cmd) self.arm_state = ArmState.SINGLE_JOINT self.last_active_arm_state = ArmState.SINGLE_JOINT self.check_joint_limits() self.pub_joint_commands.publish(self.joint_commands) # If the user is not pressing or toggling a button, then stop the robot else: if (self.arm_state != ArmState.ROBOT_POSE_CONTROL): self.arm_state = ArmState.STOPPED
def __init__(self, robot_name, mrd=None, robot_model=None, moving_time=2.0, accel_time=0.3, gripper_pressure=0.5, use_time=True): rospy.init_node(robot_name + "_robot_manipulation") # Initialize ROS Node print("waiting for get robot info...") rospy.wait_for_service(robot_name + "/get_robot_info") # Wait for ROS Services to become available print("waiting for set operating modes...") rospy.wait_for_service(robot_name + "/set_operating_modes") print("waiting for set motor register values...") rospy.wait_for_service(robot_name + "/set_motor_register_values") srv_robot_info = rospy.ServiceProxy(robot_name + "/get_robot_info", RobotInfo) # Create Service Proxies self.srv_set_op = rospy.ServiceProxy(robot_name + "/set_operating_modes", OperatingModes) self.srv_set_register = rospy.ServiceProxy(robot_name + "/set_motor_register_values", RegisterValues) self.srv_get_register = rospy.ServiceProxy(robot_name + "/get_motor_register_values", RegisterValues) self.resp = srv_robot_info() # Get Robot Info like joint limits self.use_time = use_time # Determine if 'Drive Mode' is set to 'Time' or 'Velocity' self.set_trajectory_time(moving_time, accel_time) # Change the Profile Velocity/Acceleration Registers in the Arm motors self.joint_indx_dict = dict(zip(self.resp.joint_names, range(self.resp.num_single_joints))) # Map joint names to their respective indices in the joint limit lists self.joint_states = None # Holds the current joint states of the arm self.js_mutex = threading.Lock() # Mutex to prevent writing/accessing the joint states variable at the same time self.pub_joint_commands = rospy.Publisher(robot_name + "/joint/commands", JointCommands, queue_size=100) # ROS Publisher to command the arm self.pub_single_command = rospy.Publisher(robot_name + "/single_joint/command", SingleCommand, queue_size=100) # ROS Publisher to command a specified joint self.sub_joint_states = rospy.Subscriber(robot_name + "/joint_states", JointState, self.joint_state_cb) # ROS Subscriber to get the current joint states while (self.joint_states == None and not rospy.is_shutdown()): pass # Wait until the first JointState message is received if robot_model is None: robot_model = robot_name if (mrd is not None): self.robot_des = getattr(mrd, robot_model) # Contains the Modern Robotics description of the desired arm model self.gripper_moving = False # When in PWM mode, False means the gripper PWM is 0; True means the gripper PWM in nonzero self.gripper_command = Float64() # ROS Message to hold the gripper PWM command self.set_gripper_pressure(gripper_pressure) # Maps gripper pressure to a PWM range self.gripper_index = self.joint_states.name.index("left_finger") # Index of the 'left_finger' joint in the JointState message self.initial_guesses = [[0.0] * self.resp.num_joints for i in range(3)] # Guesses made up of joint values to seed the IK function self.initial_guesses[1][0] = np.deg2rad(-120) self.initial_guesses[2][0] = np.deg2rad(120) self.pub_gripper_command = rospy.Publisher(robot_name + "/gripper/command", Float64, queue_size=100) # ROS Publisher to command the gripper self.pub_joint_traj = rospy.Publisher(robot_name + "/arm_controller/joint_trajectory", JointTrajectory, queue_size=100) # ROS Pubilsher to command Cartesian trajectories self.waist_index = self.joint_states.name.index("waist") # Index of the 'waist' joint in the JointState 'name' list self.joint_positions = list(self.joint_states.position[self.waist_index:(self.resp.num_joints+self.waist_index)]) # Holds the desired joint positions self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.joint_positions) # Transformation matrix describing the pose of the end-effector w.r.t. the Space frame tmr_controller = rospy.Timer(rospy.Duration(0.02), self.controller) # ROS Timer to check gripper position if self.use_time: rospy.loginfo("\nRobot Name: %s\nRobot Model: %s\nMoving Time: %.2f seconds\nAcceleration Time: %.2f seconds\nGripper Pressure: %d%%\nDrive Mode: Time-Based-Profile" % (robot_name, robot_model, moving_time, accel_time, gripper_pressure * 100)) else: rospy.loginfo("\nRobot Name: %s\nRobot Model: %s\nMax Velocity: %d \nMax Acceleration: %d\nGripper Pressure: %d%%\nDrive Mode: Velocity-Based-Profile" % (robot_name, robot_model, moving_time, accel_time, gripper_pressure * 100)) rospy.sleep(1) # Give time for the ROS Publishers to get set up
def __init__(self, core, robot_model, group_name, moving_time=2.0, accel_time=0.3): self.core = core self.group_info = self.core.srv_get_info("group", group_name) if (self.group_info.profile_type != "time"): rospy.logerr("Please set the group's 'profile type' to 'time'.") if (self.group_info.mode != "position"): rospy.logerr("Please set the group's 'operating mode' to 'position'.") self.robot_des = getattr(mrd, robot_model) self.initial_guesses = [[0.0] * self.group_info.num_joints for i in range(3)] self.initial_guesses[1][0] = np.deg2rad(-120) self.initial_guesses[2][0] = np.deg2rad(120) self.moving_time = None self.accel_time = None self.group_name = group_name self.joint_commands = [] self.rev = 2 * math.pi for name in self.group_info.joint_names: self.joint_commands.append(self.core.joint_states.position[self.core.js_index_map[name]]) self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.joint_commands) self.set_trajectory_time(moving_time, accel_time) self.info_index_map = dict(zip(self.group_info.joint_names, range(self.group_info.num_joints))) print("Arm Group Name: %s\nMoving Time: %.2f seconds\nAcceleration Time: %.2f seconds\nDrive Mode: Time-Based-Profile" % (group_name, moving_time, accel_time)) print("Initialized InterbotixArmXSInterface!\n")
def get_transformation(self, tracker_configuration): return mr.FKinSpace(self.end_effector_at_zero_position, self.screws, tracker_configuration.to_theta_list())
print("Ex 1: Determina the end-effector zero configuration M") M = np.array([[1, 0, 0, 3.73], [0, 1, 0, 0], [0, 0, 1, 2.73], [0, 0, 0, 1]]) print(M) # [[1,0,0,3.73],[0,1,0,0],[0,0,1,2.73],[0,0,0,1]] print("Ex 2: Determine the screw axes Si") Ss = np.array([[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 0], [1, 0, 0, 0, 0, 1], [0, 0, 1, -0.73, 0, 0], [-1, 0, 0, 0, 0, -3.73], [0, 1, 2.73, 3.73, 1, 0]]) print(Ss) # [[0,0,0,0,0,0],[0,1,1,1,0,0],[1,0,0,0,0,1],[0,0,1,-0.73,0,0],[-1,0,0,0,0,-3.73],[0,1,2.73,3.73,1,0]] print("Ex 3: Determine the screw aes Bi") Bs = np.array([[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 0], [1, 0, 0, 0, 0, 1], [0, 2.73, 3.73, 2, 0, 0], [2.73, 0, 0, 0, 0, 0], [0, -2.73, -1, 0, 1, 0]]) print(Bs) # [[0,0,0,0,0,0],[0,1,1,1,0,0],[1,0,0,0,0,1],[0,2.73,3.73,2,0,0],[2.73,0,0,0,0,0],[0,-2.73,-1,0,1,0]] print("Ex 4: Find the end-effector configuration T using FKinSpace") theta = np.array( [-math.pi / 2, math.pi / 2, math.pi / 3, -math.pi / 4, 1, math.pi / 6]) T_space = np.around(mr.FKinSpace(M, Ss, theta), 2) print(T_space) # [[0.5,0.87,0,1],[0.22,-0.13,-0.97,-1.9],[-0.84,0.48,-0.26,-4.5],[0,0,0,1]] print("Ex 5: Find the end-effector configuration T using FKinBody") T_body = np.around(mr.FKinBody(M, Bs, theta), 2) print(T_body) # [[0.5,0.87,0,1],[0.22,-0.13,-0.97,-1.9],[-0.84,0.48,-0.26,-4.5,],[0,0,0,1]]
import modern_robotics as mr import numpy as np import math M = np.array([[1, 0, 0, 3.732], [0, 1, 0, 0], [0, 0, 1, 2.732], [0, 0, 0, 1]]) thetalist = np.array( [-math.pi / 2, math.pi / 2, math.pi / 3, -math.pi / 4, 1, math.pi / 6]) Slist = np.array([[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 0], [1, 0, 0, 0, 0, 1], [0, 0, 1, -0.732, 0, 0], [-1, 0, 0, 0, 0, -3.732], [0, 1, -2.732, 3.732, 1, 0]]) Blist = np.array([[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 0], [1, 0, 0, 0, 0, 1], [0, -2.732, 3.732, 2, 0, 0], [2.732, 0, 0, 0, 0, 0], [0, 2.732, -1, 0, -1, 0]]) S = mr.FKinSpace(M, Slist, thetalist) B = mr.FKinBody(M, Blist, thetalist) print(S) print(B)
def Tsh(self, thetaList): thetaList = thetaList.transpose() return mr.FKinSpace(self.M, self.Slist, thetaList)
def getPoseFromJoints(thetas): M = transformation_data.M S = transformation_data.S T_pose = mr.FKinSpace(M, S, thetas) return T_pose
def p4(robot): print('P4') thetas = np.array( [-math.pi / 2, math.pi / 2, math.pi / 3, -math.pi / 4, 1, math.pi / 6]) T = mr.FKinSpace(robot.M, robot.Slist, thetas) print(T)
def controller(self, event): with self.js_mutex: js_msg = self.joint_states.position with self.mutex: if ((self.gripper_state == GripperState.OPEN and js_msg[-2] >= self.resp.upper_gripper_limit) or (self.gripper_state == GripperState.CLOSE and js_msg[-2] <= self.resp.lower_gripper_limit)): gripper_cmd = Float64() gripper_cmd.data = 0 self.pub_gripper_command.publish(gripper_cmd) self.gripper_state = GripperState.IDLE if (self.arm_state == ArmState.ROBOT_POSE_CONTROL): for i in range(self.num_joints): self.joint_commands.cmd[i] = self.controllers[i].compute_control(self.angles[i], js_msg[i]) if (sum(map(abs, self.joint_commands.cmd)) < 0.1): self.arm_state = ArmState.STOPPED if (self.arm_state == ArmState.VELOCITY_IK): # calculate the latest T_ssh (transform of the 'shoulder_link' w.r.t. the 'Space' frame) self.yaw_to_rotation_matrix(js_msg[0]) # calculate the latest T_sb (transform of the end-effector w.r.t. the 'Space' frame) T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, js_msg[:self.num_joints]) # Transform T_sb to get the end-effector w.r.t. the 'shoulder_link' frame. T_shb = np.dot(mr.TransInv(self.T_ssh), T_sb) # Vsh holds the desired twist w.r.t the 'shoulder_link' frame. The 'shoulder_link' frame # is used since its 'X-axis' is always aligned with the 'X-axis' of the end-effector. Additionally, # its 'Z-axis' always points straight up. Thus, it's easy to think of twists in this frame. Vsh = self.twist # If the end-effector is only doing horizontal movement... if (self.velocity_x_ik_only == True): # Calculate the error in the current end-effector pose w.r.t the initial end-effector pose. err = np.dot(mr.TransInv(T_sb), self.ee_reference) # convert this pose error into a twist w.r.t the end-effector frame Vb_err = mr.se3ToVec(mr.MatrixLog6(err)) # transform this twist into the 'shoulder_link' frame as this is the frame that the desired twist is in Vsh_err = np.dot(mr.Adjoint(T_shb), Vb_err) # Set Vx to 0 and use the Vx value in self.twist instead Vsh_err[3] = 0 Vsh = np.add(Vsh_err, self.twist) # The whole process above is done to account for gravity. If the desired twist alone was converted to # joint velocities, over time the end-effector would sag. Thus, by adding in the 'error' twist, the # end-effector will track the initial height much more accurately. # Convert the twist from the 'shoulder_link' frame to the 'Space' frame using the Adjoint Vs = np.dot(mr.Adjoint(self.T_ssh), Vsh) # Calculte the Space Jacobian js = mr.JacobianSpace(self.robot_des.Slist, js_msg[:self.num_joints]) # Calculate the joint velocities needed to achieve Vsh qdot = np.dot(np.linalg.pinv(js), Vs) # If any of the joint velocities violate their velocity limits, scale the twist by that amount scaling_factor = 1.0 for x in range(self.num_joints): if (abs(qdot[x]) > self.resp.velocity_limits[x]): sample_factor = abs(qdot[x])/self.resp.velocity_limits[x] if (sample_factor > scaling_factor): scaling_factor = sample_factor if (scaling_factor != 1.0): qdot[:] = [x / scaling_factor for x in qdot] self.joint_commands.cmd = qdot if (self.arm_state != ArmState.IDLE and self.arm_state != ArmState.STOPPED): self.check_joint_limits() if (self.arm_state == ArmState.STOPPED): # Send a speed of 0 rad/s to each joint - effectively, stopping all joints self.joint_commands.cmd = [0] * self.num_joints # Publish the joint_commands message if (self.arm_state != ArmState.IDLE and self.arm_state != ArmState.SINGLE_JOINT or self.stop_single_joint == True): self.pub_joint_commands.publish(self.joint_commands) if (self.stop_single_joint == True): self.stop_single_joint = False if (self.arm_state == ArmState.STOPPED): self.arm_state = ArmState.IDLE
def __init__(self): # Initialization parameters to control the Interbotix Arm rospy.wait_for_service("get_robot_info") srv_robot_info = rospy.ServiceProxy( "get_robot_info", RobotInfo ) # ROS Service to get joint limit information among other things self.resp = srv_robot_info( ) # Store the robot information in this variable self.joint_indx_dict = dict( zip(self.resp.joint_names, range(self.resp.num_single_joints)) ) # Map each joint-name to their respective index in the joint_names list (which conveniently matches their index in the joint-limit lists) self.joy_msg = ArmJoyControl( ) # Incoming message coming from the 'joy_control' node self.arm_model = rospy.get_param("~robot_name") # Arm-model type self.num_joints = self.resp.num_joints # Number of joints in the arm self.speed_max = 3.0 # Max scaling factor when bumping up joint speed self.gripper_pwm = 200 # Initial gripper PWM value self.gripper_moving = False # Boolean that is set to 'True' if the gripper is moving - 'False' otherwise self.gripper_command = Float64( ) # Float64 message to be sent to the 'gripper' joint self.gripper_index = self.num_joints + 1 # Index of the 'left_finger' joint in the JointState message self.follow_pose = True # True if going to 'Home' or 'Sleep' pose self.use = False self.joint_states = None # Holds the most up-to-date JointState message self.js_mutex = threading.Lock( ) # Mutex to make sure that 'self.joint_states' variable isn't being modified and read at the same time self.joint_commands = JointCommands( ) # JointCommands message to command the arm joints as a group self.target_positions = list( self.resp.sleep_pos) # Holds the 'Sleep' or 'Home' joint values self.robot_des = getattr( mrd, self.arm_model) # Modern Robotics robot description self.joy_speeds = { "course": 2.0, "fine": 2.0, "current": 2.0 } # Dictionary containing the desired joint speed scaling factors self.pub_joint_commands = rospy.Publisher( "joint/commands", JointCommands, queue_size=100, latch=True) # ROS Publisher to command joint positions [rad] self.pub_gripper_command = rospy.Publisher( "gripper/command", Float64, queue_size=100) # ROS Publisher to command gripper PWM values self.is_use = rospy.Subscriber("/vx300s/joy", Joy, self.check_cb) self.sub_joy_commands = rospy.Subscriber( "joy/commands", ArmJoyControl, self.joy_control_cb) # ROS Subscriber to get the joystick commands self.sub_joint_states = rospy.Subscriber( "joint_states", JointState, self.joint_state_cb ) # ROS Subscriber to get the current joint states while (self.joint_states == None and not rospy.is_shutdown()): pass # Wait until we know the current joint values of the robot self.joint_positions = list( self.joint_states.position[:self.num_joints] ) # Holds the desired joint positions for the robot arm at any point in time self.yaw = 0.0 # Holds the desired 'yaw' of the end-effector w.r.t. the 'base_link' frame self.T_sy = np.identity( 4 ) # Transformation matrix of a virtual frame with the same x, y, z, roll, and pitch values as 'base_link' but containing the 'yaw' of the end-effector self.T_sb = mr.FKinSpace( self.robot_des.M, self.robot_des.Slist, self.resp.sleep_pos ) # Transformation matrix of the end-effector w.r.t. the 'base_link' frame self.T_yb = np.dot( mr.TransInv(self.T_sy), self.T_sb) # Transformation matrix of the end-effector w.r.t. T_sy tmr_controller = rospy.Timer( rospy.Duration(0.02), self.controller) # ROS Timer to control the Interbotix Arm
def joy_control_cb(self, msg): if (self.use == False): self.joy_msg = msg # check gripper_cmd if (self.joy_msg.gripper_cmd != 0): with self.js_mutex: gripper_pos = self.joint_states.position[ self.gripper_index] if (self.joy_msg.gripper_cmd == ArmJoyControl.GRIPPER_OPEN and gripper_pos < self.resp.upper_gripper_limit): self.gripper_command.data = self.gripper_pwm elif (self.joy_msg.gripper_cmd == ArmJoyControl.GRIPPER_CLOSE and gripper_pos > self.resp.lower_gripper_limit): self.gripper_command.data = -self.gripper_pwm self.pub_gripper_command.publish(self.gripper_command) self.gripper_moving = True # check robot_pose if (self.joy_msg.robot_pose != 0): if (self.joy_msg.robot_pose == ArmJoyControl.HOME_POSE): self.target_positions = list(self.resp.home_pos) elif (self.joy_msg.robot_pose == ArmJoyControl.SLEEP_POSE): self.target_positions = list(self.resp.sleep_pos) self.follow_pose = True # reset all transforms self.yaw = 0.0 self.T_sy[:2, :2] = self.yaw_to_rotation_matrix(self.yaw) self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, self.target_positions) self.T_yb = np.dot(mr.TransInv(self.T_sy), self.T_sb) # check speed_cmd if (self.joy_msg.speed_cmd != 0): if (self.joy_msg.speed_cmd == ArmJoyControl.SPEED_INC and self.joy_speeds["current"] < self.speed_max): self.joy_speeds["current"] += 0.25 elif (self.joy_msg.speed_cmd == ArmJoyControl.SPEED_DEC and self.joy_speeds["current"] > 1): self.joy_speeds["current"] -= 0.25 rospy.loginfo("Current scaling factor is %.2f." % self.joy_speeds["current"]) # check toggle_speed_cmd if (self.joy_msg.toggle_speed_cmd != 0): if (self.joy_msg.toggle_speed_cmd == ArmJoyControl.SPEED_COURSE ): self.joy_speeds["fine"] = self.joy_speeds["current"] self.joy_speeds["current"] = self.joy_speeds["course"] rospy.loginfo("Switched to Course Control") elif (self.joy_msg.toggle_speed_cmd == ArmJoyControl.SPEED_FINE ): self.joy_speeds["course"] = self.joy_speeds["current"] self.joy_speeds["current"] = self.joy_speeds["fine"] rospy.loginfo("Switched to Fine Control") # check gripper_pwm_cmd if (self.joy_msg.gripper_pwm_cmd != 0): if (self.joy_msg.gripper_pwm_cmd == ArmJoyControl.GRIPPER_PWM_INC and self.gripper_pwm < 350): self.gripper_pwm += 25 elif (self.joy_msg.gripper_pwm_cmd == ArmJoyControl.GRIPPER_PWM_DEC and self.gripper_pwm > 150): self.gripper_pwm -= 25 rospy.loginfo("Current PWM command is %d." % self.gripper_pwm)
import modern_robotics as mr import numpy as np import math M = np.array([[1, 0, 0, 3.732], [0, 1, 0, 0], [0, 0, 1, 2.732], [0, 0, 0, 1]]) Slist = np.array([[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 0], [1, 0, 0, 0, 0, 1], [0, 0, 1, -0.732, 0, 0], [-1, 0, 0, 0, 0, -3.732], [0, 1, 2.732, 3.732, 1, 0]]) Blist = np.array([[0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 0], [1, 0, 0, 0, 0, 1], [0, 2.732, 3.732, 2, 0, 0], [2.732, 0, 0, 0, 0, 0], [0, -2.732, -1, 0, 1, 0]]) thetaList = np.array( [-math.pi / 2, math.pi / 2, math.pi / 3, -math.pi / 4, 1, math.pi / 6]) R = mr.FKinSpace(M, Slist, thetaList) T = mr.FKinBody(M, Blist, thetaList) print R print T
def joy_control_cb(self, msg): with self.js_mutex: js_msg = self.joint_states.position with self.mutex: # Check if the gripper pwm or state should be changed if (self.resp.use_gripper == True): if (msg.gripper_pwm_cmd != 0): self.set_gripper_pwm(msg.gripper_pwm_cmd) if (msg.gripper_cmd != 0): self.set_gripper_cmd(msg.gripper_cmd, js_msg[-2]) if (msg.arm_speed_cmd != 0 or msg.arm_toggle_speed_cmd != 0): self.set_arm_speeds(msg.arm_speed_cmd, msg.arm_toggle_speed_cmd) # Check if the arm should go to a 'Home' or 'Sleep' pose if (msg.robot_pose != 0): self.set_robot_pose(msg.robot_pose) self.arm_state = ArmState.ROBOT_POSE_CONTROL self.last_active_arm_state = ArmState.ROBOT_POSE_CONTROL # Check if the end-effector should move horizontally and/or vertically... # If the arm is 5dof or less, then ignore 'ee_y_cmd' commands from the joystick controller elif ( ((msg.ee_x_cmd != 0 or msg.ee_y_cmd != 0 or msg.ee_z_cmd != 0) and self.num_joints >= 6) or ((msg.ee_x_cmd != 0 or msg.ee_z_cmd != 0) and self.num_joints < 6)): self.set_velocity_ik(msg.ee_x_cmd, msg.ee_y_cmd, msg.ee_z_cmd) if (msg.ee_z_cmd == 0): # compensate for drift due to gravity by creating a snapshot of the end-effector pose [T_sb] when # coming from SINGLE_JOINT, ROBOT_POSE_CONTROL, or VELOCITY_IK containing a z-component; the controller # will track the desired height, orieintation, (and y-position if less than 6dof) and correct itself accordingly if not (self.last_active_arm_state == ArmState.VELOCITY_IK and self.velocity_horz_ik_only == True): self.ee_reference = mr.FKinSpace( self.robot_des.M, self.robot_des.Slist, js_msg[:self.num_joints]) # if the arm has 6dof and the arm is already in VELOCITY_IK mode... if (self.num_joints >= 6 and self.last_active_arm_state == ArmState.VELOCITY_IK): # create a snapshot of the y-position if the end-effector is only moving in the x-direction to prevent 'y' drift if (msg.ee_x_cmd != 0 and msg.ee_y_cmd == 0 and self.velocity_y_ik_only == True): ee_ref = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, js_msg[:self.num_joints]) self.ee_reference[1, 3] = ee_ref[1, 3] self.velocity_y_ik_only = False self.velocity_x_ik_only = True # create a snapshot of the x-position if the end-effector is only moving in the y-direction to prevent 'x' drift elif (msg.ee_x_cmd == 0 and msg.ee_y_cmd != 0 and self.velocity_x_ik_only == True): ee_ref = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, js_msg[:self.num_joints]) self.ee_reference[0, 3] = ee_ref[0, 3] self.velocity_y_ik_only = True self.velocity_x_ik_only = False # if the arm in moving diagonally in the x-y plane without a z-component, then set both of the variables below # to True so that a snapshot can be taken if you switch to x-only or y-only control elif (msg.ee_x_cmd != 0 and msg.ee_y_cmd != 0): self.velocity_y_ik_only = True self.velocity_x_ik_only = True self.velocity_horz_ik_only = True else: self.velocity_horz_ik_only = False self.arm_state = ArmState.VELOCITY_IK self.last_active_arm_state = ArmState.VELOCITY_IK # Check if indiviual joints should be rotated elif (msg.waist_cmd != 0 or msg.wrist_angle_cmd != 0 or msg.wrist_rotate_cmd != 0): self.set_single_joint(msg.waist_cmd, msg.wrist_angle_cmd, msg.wrist_rotate_cmd) self.arm_state = ArmState.SINGLE_JOINT self.last_active_arm_state = ArmState.SINGLE_JOINT self.check_joint_limits() self.pub_joint_commands.publish(self.joint_commands) # If the user is not pressing or toggling a button, then stop the robot else: if (self.arm_state != ArmState.ROBOT_POSE_CONTROL): self.arm_state = ArmState.STOPPED
V_s_list[4, 2] = 1 S_list = np.array(np.concatenate((W_list, V_s_list), axis=1)).T print("\n") print("Question 2:\n", np.array2string(S_list, separator=',')) ######### Question#3 ############# r_b_list = np.array([ [-2.73, 0, -2.73], #r1 [-2.73, 0, -2.72], #r2 [0, 0, -3.73], #r3 [-1, 0, -2], #r4 [0, 0, 0], #r5 [0, 0, 0] #r6 ]) V_b_list = np.array(np.cross(W_list, (-r_b_list))) V_b_list[4, 2] = 1 B_list = np.array(np.concatenate((W_list, V_b_list), axis=1)).T print("\n") print("Question 3:\n", np.array2string(B_list, separator=',')) ######### Question#4 ############# Theta_list = np.array([[-pi / 2, pi / 2, pi / 3, -pi / 4, 1, pi / 6]]) Fk = Mr.FKinSpace(M, S_list, Theta_list) Fk_s = np.around(Fk, 2) print("Question #4:\n", np.array2string(Fk_s, separator=',')) ######### Question#4 ############# Fk = Mr.FKinBody(M, B_list, Theta_list) Fk_b = np.around(Fk, 2) print("Question #5:\n", np.array2string(Fk_b, separator=','))
def main(): clientID = startSimulation() # Handles joint_handles = [-1, -1, -1, -1, -1, -1] for i in range(0, 6): joint_handles[i] = sim.simxGetObjectHandle(clientID, 'UR3_joint' + str(i + 1), sim.simx_opmode_blocking)[1] base_handle = sim.simxGetObjectHandle(clientID, "UR3", sim.simx_opmode_blocking)[1] end_effector_handle = sim.simxGetObjectHandle(clientID, "UR3_link7_visible", sim.simx_opmode_blocking)[1] end_effector_wrt_base = sim.simxGetObjectPosition( clientID, end_effector_handle, base_handle, sim.simx_opmode_streaming)[1] end_effector_quat_wrt_base = sim.simxGetObjectQuaternion( clientID, end_effector_handle, base_handle, sim.simx_opmode_streaming)[1] time.sleep(0.5) # Get end effector position and rotation wrt base end_effector_wrt_base = sim.simxGetObjectPosition( clientID, end_effector_handle, base_handle, sim.simx_opmode_oneshot_wait)[1] end_effector_quat_wrt_base = sim.simxGetObjectQuaternion( clientID, end_effector_handle, base_handle, sim.simx_opmode_oneshot_wait)[1] R_end_effector_wrt_base = Rotation.from_quat( end_effector_quat_wrt_base).as_matrix() P_end_effector_wrt_base = end_effector_wrt_base M = mr.RpToTrans(R_end_effector_wrt_base, P_end_effector_wrt_base) print(M) # Forward Kinematics Slist = getSlist(clientID, joint_handles, base_handle) theta = [ degToRad(180), degToRad(0), degToRad(0), degToRad(0), degToRad(0), degToRad(0) ] T_endeff_in_base = mr.FKinSpace(M, Slist.T, theta) print(T_endeff_in_base) success = False while (not success): theta_new, success = mr.IKinSpace(Slist.T, M, T_endeff_in_base, [ random.random(), random.random(), random.random(), random.random(), random.random(), random.random() ], 0.01, 0.01) time.sleep(1) move.setTargetPosition(clientID, joint_handles, theta_new) errorCode = 1 while (errorCode): errorCode, end_effector_wrt_base = sim.simxGetObjectPosition( clientID, end_effector_handle, base_handle, sim.simx_opmode_oneshot_wait) print(end_effector_wrt_base) time.sleep(1) move.setTargetPosition(clientID, joint_handles, [0, 0, 0, 0, 0, 0]) endSimulation(clientID) return 1
def controller(self, event): with self.js_mutex: js_msg = self.joint_states.position with self.mutex: if ((self.gripper_state == GripperState.OPEN and js_msg[-2] >= self.resp.upper_gripper_limit) or (self.gripper_state == GripperState.CLOSE and js_msg[-2] <= self.resp.lower_gripper_limit)): gripper_cmd = Float64() gripper_cmd.data = 0 self.pub_gripper_command.publish(gripper_cmd) self.gripper_state = GripperState.IDLE if (self.arm_state == ArmState.ROBOT_POSE_CONTROL): for i in range(self.num_joints): self.joint_commands.cmd[i] = self.controllers[ i].compute_control(self.angles[i], js_msg[i]) if (sum(map(abs, self.joint_commands.cmd)) < 0.1): self.arm_state = ArmState.STOPPED if (self.arm_state == ArmState.VELOCITY_IK): # calculate the latest T_sb (transform of the end-effector w.r.t. the 'Space' frame) T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, js_msg[:self.num_joints]) # if the arm is at least 6 dof, then calculate the 'yaw' of T_sy from T_sb # Note that this only works well if the end-effector is not pitched at +/- 90 deg # If the arm is less than 6 dof, then calculate the 'yaw' of T_sy from the 'waist' joint as this will always have the # same 'yaw' as T_sb; there will also not be any issue if the end-effector is pitched at +/- 90 deg if (self.num_joints >= 6): yaw = np.arctan2(T_sb[1, 0], T_sb[0, 0]) else: yaw = js_msg[self.joint_indx_dict["waist"]] self.yaw_to_rotation_matrix(yaw) # Transform T_sb to get the end-effector w.r.t. T_sy T_yb = np.dot(mr.TransInv(self.T_sy), T_sb) # Vy holds the desired twist w.r.t T_sy. This frame is used since its 'X-axis' # is always aligned with the 'X-axis' of the end-effector. Additionally, its # 'Z-axis' always points straight up. Thus, it's easy to think of twists in this frame. Vy = self.twist # If the end-effector is only doing horizontal movement... if (self.velocity_horz_ik_only == True): # Calculate the error in the current end-effector pose w.r.t the initial end-effector pose. err = np.dot(mr.TransInv(T_sb), self.ee_reference) # convert this pose error into a twist w.r.t the end-effector frame Vb_err = mr.se3ToVec(mr.MatrixLog6(err)) # transform this twist into the T_sy frame as this is the frame that the desired twist is in Vy_err = np.dot(mr.Adjoint(T_yb), Vb_err) # if moving in the x-direction, set Vx in Vy_err to 0 and use the Vx value in self.twist instead if (self.twist[3] != 0): Vy_err[3] = 0 # if moving in the y-direction, set Vy in Vy_err to 0 and use the Vy value in self.twist instead if (self.num_joints >= 6 and self.twist[4] != 0): Vy_err[4] = 0 # if moving in the x-y plane, update the 'x' and 'y' values in ee_ref so that Vy_err is calculated correctly if (self.num_joints >= 6 and self.twist[3] != 0 and self.twist[4] != 0): self.ee_reference[0, 3] = T_sb[0, 3] self.ee_reference[1, 3] = T_sb[1, 3] Vy = np.add(Vy_err, self.twist) # The whole process above is done to account for gravity. If the desired twist alone was converted to # joint velocities, over time the end-effector would sag. Thus, by adding in the 'error' twist, the # end-effector will track the initial height much more accurately. # Convert the twist from the T_sy frame to the 'Space' frame using the Adjoint Vs = np.dot(mr.Adjoint(self.T_sy), Vy) # Calculte the Space Jacobian js = mr.JacobianSpace(self.robot_des.Slist, js_msg[:self.num_joints]) # Calculate the joint velocities needed to achieve Vsh qdot = np.dot(np.linalg.pinv(js), Vs) # If any of the joint velocities violate their velocity limits, scale the twist by that amount scaling_factor = 1.0 for x in range(self.num_joints): if (abs(qdot[x]) > self.resp.velocity_limits[x]): sample_factor = abs( qdot[x]) / self.resp.velocity_limits[x] if (sample_factor > scaling_factor): scaling_factor = sample_factor if (scaling_factor != 1.0): qdot[:] = [x / scaling_factor for x in qdot] self.joint_commands.cmd = qdot if (self.arm_state != ArmState.IDLE and self.arm_state != ArmState.STOPPED): self.check_joint_limits() if (self.arm_state == ArmState.STOPPED): # Send a speed of 0 rad/s to each joint - effectively, stopping all joints self.joint_commands.cmd = [0] * self.num_joints # Publish the joint_commands message if (self.arm_state != ArmState.IDLE and self.arm_state != ArmState.SINGLE_JOINT or self.stop_single_joint == True): self.pub_joint_commands.publish(self.joint_commands) if (self.stop_single_joint == True): self.stop_single_joint = False if (self.arm_state == ArmState.STOPPED): self.arm_state = ArmState.IDLE
#--------------------------------------------------------Main Program-------------------------------------------------------------- #Desired Configuration: Ti = np.array([[-1,0,0,0], [0,1,0,200], [0,0,-1,0], [0,0,0,1]]) Tf = np.array([[-1,0,0,100], [0,1,0,200], [0,0,-1,300], [0,0,0,1]]) Fk = mr.FKinSpace(Robot.M,Robot.Slist.T,[np.deg2rad(90),np.deg2rad(0),np.deg2rad(90),np.deg2rad(90),np.deg2rad(90),np.deg2rad(0)]) Ik = mr.IKinSpace(Robot.Slist.T,Robot.M,Ti,BestTheta0(Ti[0][3],Ti[1][3],Ti[2][3],206,239),eomg=0.0001,ev=0.0001) animarTrajetoria(Ti,Tf,10) for i in range(6): if Ik[0][i] > 0: while Ik[0][i] > np.pi*2: Ik[0][i] = Ik[0][i] - np.pi*2 if Ik[0][i] < 0: while Ik[0][i] < -np.pi*2: Ik[0][i] = Ik[0][i] + np.pi*2 for i in range(6): Ik[0][i] = round(np.rad2deg(Ik[0][i]),0) Ik[0][i] = float(Ik[0][i])