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