def move(twist_msg):
# have to switch the order of linear and angular velocities in twist
# message so that it comes in the form needed by the modern_robotics library
end_effector_vel = np.zeros(6)
end_effector_vel[0] = 0 #twist_msg.angular.x
end_effector_vel[1] = 0 #twist_msg.angular.y
end_effector_vel[2] = 0 #twist_msg.angular.z
end_effector_vel[3] = twist_msg.linear.x
end_effector_vel[4] = twist_msg.linear.y
end_effector_vel[5] = twist_msg.linear.z
arm_angles_dict = arm.joint_angles()
thetalist0 = [] # initial guess at joint angles
thetalist0.append(arm_angles_dict['right_j0'])
thetalist0.append(arm_angles_dict['right_j1'])
thetalist0.append(arm_angles_dict['right_j2'])
thetalist0.append(arm_angles_dict['right_j3'])
thetalist0.append(arm_angles_dict['right_j4'])
thetalist0.append(arm_angles_dict['right_j5'])
thetalist0.append(arm_angles_dict['right_j6'])
thetalist0 = np.array(thetalist0)
J = mr.JacobianSpace(Slist, thetalist0)
pinv_J = np.linalg.pinv(J)
# print("The shape of the end effector velocity vector is {}".format(end_effector_vel.shape))
# print("The shape of the Jacobian Pseudo Inverse matrix is {}".format(pinv_J.shape))
joint_vels = np.dot(pinv_J, end_effector_vel)
# velocities need to be passed in as a dictionary
joint_vels_dict = {}
for i, vel in enumerate(joint_vels):
joint_vels_dict['right_j' + str(i)] = vel
# set joint velocities
arm.set_joint_velocities(joint_vels_dict)
def move(self, twist_msg):
# have to switch the order of linear and angular velocities in twist
# message so that it comes in the form needed by the modern_robotics library
self.end_effector_vel[0] = 0 #twist_msg.angular.x
self.end_effector_vel[1] = 0 #twist_msg.angular.y
self.end_effector_vel[2] = 0 #twist_msg.angular.z
self.end_effector_vel[3] = twist_msg.linear.x
self.end_effector_vel[4] = twist_msg.linear.y
self.end_effector_vel[5] = twist_msg.linear.z
arm_angles_dict = self.arm.joint_angles()
thetalist0 = []
for i in range(len(arm_angles_dict)):
thetalist0.append(arm_angles_dict['right_j' + str(i)])
J = mr.JacobianSpace(self.Slist, np.array(thetalist0))
pinv_J = np.linalg.pinv(J)
# print("The shape of the end effector velocity vector is {}".format(self.end_effector_vel.shape))
# print("The shape of the Jacobian Pseudo Inverse matrix is {}".format(pinv_J.shape))
joint_vels = np.dot(pinv_J, self.end_effector_vel)
for i, vel in enumerate(joint_vels):
self.joint_vels_dict['right_j' + str(i)] = vel
# set joint velocities
self.arm.set_joint_velocities(self.joint_vels_dict)
def p3():
print('P3')
Slist = np.array([[0, 1, 0], [0, 0, 0], [1, 0, 0], [0, 0, 0], [0, 2, 1],
[0, 0, 0]])
thetalist = np.array([math.pi / 2, math.pi / 2, 1])
J_s = mr.JacobianSpace(Slist, thetalist)
print(J_s)
def p1():
print('P1')
Slist = np.array([[0, 0, 0], [0, 0, 0], [1, 1, 1], [0, 0, 0], [0, -1, -2],
[0, 0, 0]])
thetalist = np.array([0, math.pi / 4, 5 * math.pi / 4])
F_s = np.array([0, 0, 0, 2, 0, 0])
J_s = mr.JacobianSpace(Slist, thetalist)
tau = np.dot(np.transpose(J_s), F_s)
print(tau)
import numpy as np
import modern_robotics as mr
import numpy.linalg as la
from math import pi
import math
############################################
######### question #1 ######################
F_s_1 = np.array([0, 0, 0, 2, 0, 0])
w_s_list_1 = np.array([[0, 0, 1], [0, 0, 1], [0, 0, 1]])
r_s_list_1 = np.array([[0, 0, 0], [1, 0, 0], [2, 0, 0]])
V_s_list_1 = np.array(np.cross(w_s_list_1, (-r_s_list_1)))
S_list_1 = np.array(np.concatenate((w_s_list_1, V_s_list_1), axis=1)).T
theta_list_1 = np.array([0, pi / 4, 0])
J_s_1 = mr.JacobianSpace(S_list_1, theta_list_1)
taulist_1 = np.around(np.matmul(J_s_1.T, F_s_1), decimals=2)
print("Question 1:\n", np.array2string(taulist_1, separator=','))
######### question #2 ######################
w_b_list_2 = np.array([[0, 0, 0], [0, 0, 1], [0, 0, 1], [0, 0, 1]])
r_b_list_2 = np.array([[0, 0, 0], [1, 0, 0], [2, 0, 0], [3, 0, 0]])
V_b_list_2 = np.array(np.cross(w_b_list_2, (-r_b_list_2)))
B_list_2 = np.array(np.concatenate((w_b_list_2, V_b_list_2), axis=1)).T
theta_list_2 = np.array([0, 0, pi / 2, -pi / 2])
F_b_2 = np.array([0, 0, 10, 10, 10, 0])
J_b_2 = mr.JacobianBody(B_list_2, theta_list_2)
taulist_2 = np.around(np.matmul(J_b_2.T, F_b_2), decimals=2)
print("Question 2:\n", np.array2string(taulist_2, separator=','))
######### question #3 ######################
S_list_3 = np.array([[0, 1, 0], [0, 0, 0], [1, 0, 0], [0, 0, 0], [0, 2, 1],
[0, 0, 0]])
theta_list_3 = np.array([pi / 2, pi / 2, 1])
J_s_3 = mr.JacobianSpace(S_list_3, theta_list_3)
import numpy as np
import modern_robotics as mr
import math
print("Ex 1: What torques must be aplied at each of the joints?")
Fs = np.array([0, 0, 0, 2, 0, 0]) # tip generates a wrench in x direction
theta = np.array([0, math.pi / 4, 0])
Ss = np.array([[0, 0, 0], [0, 0, 0], [1, 1, 1], [0, 0, 0], [0, -1, -2],
[0, 0, 0]])
Js = mr.JacobianSpace(Ss, theta)
Ts = np.around(np.dot(Js.T, Fs), decimals=2)
print(Ts)
# [0,0,1.41]
print("Ex 2: What are the torques at each of the joints")
Fb = np.array([0, 0, 10, 10, 10, 0])
theta = np.array([0, 0, math.pi / 2, -math.pi / 2])
Bs = np.array([[0, 0, 0, 0], [0, 0, 0, 0], [1, 1, 1, 1], [0, 0, 0, 0],
[4, 3, 2, 1], [0, 0, 0, 0]])
Jb = mr.JacobianBody(Bs, theta)
Tb = np.around(np.dot(Jb.T, Fb), decimals=2)
print(Tb)
# [30,20,10,20]
print("Ex 3: Use JacobianSpace to calculate the 6x3 space Jacobian")
theta = np.array([math.pi / 2, math.pi / 2, 1])
Ss = np.array([[0, 1, 0], [0, 0, 0], [1, 0, 0], [0, 0, 0], [0, 2, 1],
[0, 0, 0]])
Js = np.around(mr.JacobianSpace(Ss, theta), decimals=2)
print(Js)
# [[0,0,0],[0,1,0],[1,0,0],[0,-2,-0],[0,0,0],[0,0,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_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
import modern_robotics as mr
import numpy as np
import sympy as sym
print("Question 1 \n")
s1 = np.array([0, 0, 0, 1, 0, 0])
s2 = np.array([0, 0, 0, 0, -1, 0])
s3 = np.array([0, 0, 0, 0, -1, 0])
s1 = np.transpose(s1)
s2 = np.transpose(s2)
s3 = np.transpose(s3)
Slist = np.array([s1, s2, s3])
JS = mr.JacobianSpace(Slist, thetalist)
print(JS)
print("\nQuestion 2 \n")
theta = np.array([[0.95], [0.98], [-0.92]])
b1 = np.array([1, 0, 0, 0, 0, 0]) # -4,0,0
b2 = np.array([1, 0, 0, 0, 0, 0]) #-6,0,0
b3 = np.array([0, 0, 0, 1, 0, 0]) # 0 -2 t
Blist = np.array([b1, b2, b3])
Blist = np.transpose(Blist)
JB = mr.JacobianBody(Blist, theta)
print(JB)
print("\n Question 3 \n")
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
def JointsVelocity(Slist,thetalist,Vs):
J = mr.JacobianSpace(Slist,thetalist)
thetaVel = np.matmul(Vs,J.T)
return thetaVel
import modern_robotics as mr import numpy as np import math Slist = np.array([[0,0,1,0,0,0],[1,0,0,0,2,0],[0,0,0,0,1,0]]).T thetaList = np.array([math.pi/2,math.pi/2,1]) Blist = np.array([[0,1,0,3,0,0],[-1,0,0,0,3,0],[0,0,0,0,0,1]]).T Js = mr.JacobianSpace(Slist, thetaList) Jb = mr.JacobianBody(Blist, thetaList) # Slist = np.array([[0, 0, 1, 0, 0.2, 0.2], # [1, 0, 0, 2, 0, 3], # [0, 1, 0, 0, 2, 1], # [1, 0, 0, 0.2, 0.3, 0.4]]).T # thetalist = np.array([0.2, 1.1, 0.1, 1.2]) Jvel = np.array([[-0.105,0,0.006,-0.045,0,0.006,0],[-0.889,0.006,0,-0.844,0.006,0,0],[0,-0.105,0.889,0,0,0,0]]) A = np.dot(Jvel, Jvel.T) eigenA = np.linalg.eig(A) print eigenA
def Js(self, thetaList):
return mr.JacobianSpace(self.Slist, thetaList)