视频演示
思维导图
截图
查找python位置
安装numpy
Home配置时末端执行器位姿
youbot定义
源码：
#main.py
import numpy as np
from TrajectoryGenerator import trajectoryGenerator
from NextState import nextState
from FeedbackControl import feedbackControl
import time
import sys
sys.path.append('./vrep/VREP_remoteAPIs')
#%% VREP control function definition
''' Arm control function of youBot '''
def VREP_armControl(vrep_sim, clientID, arm_joints_handle, desired_arm_joint_angles):
for i in range(0,5):
vrep_sim.simxSetJointPosition(clientID, arm_joints_handle[i], desired_arm_joint_angles[i], vrep_sim.simx_opmode_oneshot)
''' Wheels control function of youBot '''
def VREP_wheelsControl(vrep_sim, clientID, wheel_joints_handle, desired_wheel_positions):
for i in range(0,4):
vrep_sim.simxSetJointPosition(clientID, wheel_joints_handle[i], desired_wheel_positions[i], vrep_sim.simx_opmode_oneshot)
''' World frame control function of youBot '''
def VREP_WorldFrameControl(vrep_sim, clientID, world_joints_handle, desired_world_positions):
for i in range(0,3):
vrep_sim.simxSetJointPosition(clientID, world_joints_handle[i], desired_world_positions[i], vrep_sim.simx_opmode_oneshot)
#%% VREP initialization
try:
import sim as vrep_sim
except:
print ('--------------------------------------------------------------')
print ('"sim.py" could not be imported. This means very probably that')
print ('either "sim.py" or the remoteApi library could not be found.')
print ('Make sure both are in the same folder as this file,')
print ('or appropriately adjust the file "sim.py"')
print ('--------------------------------------------------------------')
print ('')
print ('Program started')
vrep_sim.simxFinish(-1) # just in case, close all opened connections
clientID = vrep_sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim
if clientID != -1:
print ('Connected to remote API server')
return_code, youBot_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBot', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object youBot ok.')
return_code, youBot_dummy_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBotDummy', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object youBotDummy ok.')
# Prepare initial values for four wheels
wheel_joints_handle = [-1,-1,-1,-1]; # front left, rear left, rear right, front right
return_code, wheel_joints_handle[0] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_fl', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object youBot rollingJoint_rl ok.')
return_code, wheel_joints_handle[1] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_fr', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object youBot rollingJoint_fl ok.')
return_code, wheel_joints_handle[2] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_rr', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object youBot rollingJoint_fr ok.')
return_code, wheel_joints_handle[3] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_rl', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object youBot rollingJoint_rr ok.')
# Prepare initial values for five arm joints
arm_joints_handle = [-1,-1,-1,-1,-1]
for i in range(0,5):
return_code, arm_joints_handle[i] = vrep_sim.simxGetObjectHandle(clientID, 'Joint' + str(i+1), vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object arm joint ' + str(i+1) + ' ok.')
return_code, gripper_joint_1_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBotGripperJoint1', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object gripper joint 1 ok')
return_code, gripper_joint_2_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBotGripperJoint2', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object gripper joint 2 ok')
return_code, gripper_tip_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBot_positionTip', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object gripper position tip ok')
# Desired joint positions for initialization
desired_arm_joint_angles = [0, 0, 0, 0, 0]
# Initialization all arm joints
for i in range(0,5):
vrep_sim.simxSetJointPosition(clientID, arm_joints_handle[i], desired_arm_joint_angles[i], vrep_sim.simx_opmode_blocking)
# Get world joints handles
world_joints_handle = [-1, -1, -1]
return_code, world_joints_handle[0] = vrep_sim.simxGetObjectHandle(clientID, 'World_X_Joint', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object world joint X ok.')
return_code, world_joints_handle[1] = vrep_sim.simxGetObjectHandle(clientID, 'World_Y_Joint', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object world joint Y ok.')
return_code, world_joints_handle[2] = vrep_sim.simxGetObjectHandle(clientID, 'World_Th_Joint', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object world joint Th ok.')
# Get initial and goal cube handles
return_code, cube_1_handle = vrep_sim.simxGetObjectHandle(clientID, 'Cube_1', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object cube_1 ok.')
return_code, cube_2_handle = vrep_sim.simxGetObjectHandle(clientID, 'Cube_2', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object cube_2 ok.')
return_code, cube_goal_handle = vrep_sim.simxGetObjectHandle(clientID, 'Cube_goal', vrep_sim.simx_opmode_blocking)
if (return_code == vrep_sim.simx_return_ok):
print('get object cube_goal_1 ok.')
else:
print ('Failed connecting to remote API server')
#%% ---------------------- generate trajectory and control the robot ---------------------------
# Get initial and goal cube positions
_, cube_1_position = vrep_sim.simxGetObjectPosition(clientID, cube_1_handle, -1, vrep_sim.simx_opmode_blocking)
_, cube_2_position = vrep_sim.simxGetObjectPosition(clientID, cube_2_handle, -1, vrep_sim.simx_opmode_blocking)
_, cube_goal_position = vrep_sim.simxGetObjectPosition(clientID, cube_goal_handle, -1, vrep_sim.simx_opmode_blocking)
for i in range (0,2):
# Get youBot position
_, youBot_position = vrep_sim.simxGetObjectPosition(clientID, youBot_handle, -1, vrep_sim.simx_opmode_blocking)
_, youBot_x = vrep_sim.simxGetJointPosition(clientID, world_joints_handle[0], vrep_sim.simx_opmode_blocking)
_, youBot_y = vrep_sim.simxGetJointPosition(clientID, world_joints_handle[1], vrep_sim.simx_opmode_blocking)
_, youBot_phi = vrep_sim.simxGetJointPosition(clientID, world_joints_handle[2], vrep_sim.simx_opmode_blocking)
# Get youBot arm joint angles
youBot_arm_q = [0, 0, 0, 0, 0]
for j in range(0, 5):
_, youBot_arm_q[j] = vrep_sim.simxGetJointPosition(clientID, arm_joints_handle[j], vrep_sim.simx_opmode_blocking)
# Get youBot wheel angles
youBot_wheel_angles = [0, 0, 0, 0]
for j in range(0, 4):
_, youBot_wheel_angles[j] = vrep_sim.simxGetJointPosition(clientID, wheel_joints_handle[j], vrep_sim.simx_opmode_blocking)
# Get youBot gripper position
_, youBot_gripper_position = vrep_sim.simxGetObjectPosition(clientID, gripper_tip_handle, -1, vrep_sim.simx_opmode_blocking)
# Initialize the robot's configuration
# phi x y J1 J2 J3 J4 J5 w1 w2 w3 w4 gripper
config_initial = np.array([youBot_phi, youBot_x, youBot_y, youBot_arm_q[0], youBot_arm_q[1], youBot_arm_q[2], youBot_arm_q[3], youBot_arm_q[4], youBot_wheel_angles[0], youBot_wheel_angles[1], youBot_wheel_angles[2], youBot_wheel_angles[3], 0]).reshape(1,13)
# Generate trajectory
T_se_initial = np.array([[0,0,1,youBot_gripper_position[0]],[0,1,0,youBot_gripper_position[1]],[-1,0,0,youBot_gripper_position[2]],[0,0,0,1]])
if i == 0:
T_sc_initial = np.array([[1,0,0,cube_1_position[0]],[0,1,0,cube_1_position[1]],[0,0,1,cube_1_position[2]],[0,0,0,1]])
T_sc_final = np.array([[0,1,0,cube_goal_position[0]],[-1,0,0,cube_goal_position[1]],[0,0,1, cube_goal_position[2]],[0,0,0,1]])
else:
T_sc_initial = np.array([[1,0,0,cube_2_position[0]],[0,1,0,cube_2_position[1]],[0,0,1,cube_2_position[2]],[0,0,0,1]])
T_sc_final = np.array([[0,1,0,cube_goal_position[0]],[-1,0,0,cube_goal_position[1]],[0,0,1, cube_goal_position[2] + 0.05],[0,0,0,1]])
gripper_pose = np.pi/3 # change orientation of the gripper, default value is pi/4
T_ce_grasp = np.array([[-np.sin(gripper_pose), 0, np.cos(gripper_pose),0],[0,1,0,0],[-np.cos(gripper_pose), 0, -np.sin(gripper_pose),0],[0,0,0,1]])
T_ce_standoff = np.array([[-np.sin(gripper_pose), 0, np.cos(gripper_pose),0],[0,1,0,0],[-np.cos(gripper_pose), 0, -np.sin(gripper_pose),0.1],[0,0,0,1]])
k = 1
T_se_post = trajectoryGenerator(T_se_initial,T_sc_initial,T_sc_final,T_ce_grasp,T_ce_standoff,k)
# set to use joint limits
joint_limit = 0 #不用关节限制。若使用设置为1，在feedbackControl模块中将机械臂J3,J4 在> -0.1 置为0
# set Kp, Ki and dt
Kp = 2.2
Ki = 0
dt = 0.01
#%% ------------------------------------- main loop --------------------------------------
desired_wheel_positions = [0,0,0,0]
desired_world_positions = [0,0,0]
reference_trajectory = []
reference_trajectory.append(config_initial)
new_config = config_initial
print('begin main control loop ...')
for i in np.arange(np.shape(T_se_post)[0]-1):
# write down Xd
Xd = np.array([[T_se_post[i,0],T_se_post[i,1],T_se_post[i,2],T_se_post[i,9]],[T_se_post[i,3],T_se_post[i,4],T_se_post[i,5],T_se_post[i,10]],[T_se_post[i,6],T_se_post[i,7],T_se_post[i,8],T_se_post[i,11]],[0,0,0,1]])
# write down Xd_next
Xd_next = np.array([[T_se_post[i+1,0],T_se_post[i+1,1],T_se_post[i+1,2],T_se_post[i+1,9]],[T_se_post[i+1,3],T_se_post[i+1,4],T_se_post[i+1,5],T_se_post[i+1,10]],[T_se_post[i+1,6],T_se_post[i+1,7],T_se_post[i+1,8],T_se_post[i+1,11]],[0,0,0,1]])
# compute speeds and X_err
V, speeds, X_err = feedbackControl(new_config, Xd, Xd_next, Kp, Ki, dt, joint_limit)
# adjust the order of wheels speeds and joints speeds
speeds = np.array([speeds[4], speeds[5], speeds[6], speeds[7], speeds[8], speeds[0], speeds[1], speeds[2], speeds[3]]).reshape(1,9)
speeds_max = 1000
# compute new configuration
new_config = nextState(new_config,speeds,dt,speeds_max)
new_config = np.append(new_config,[[T_se_post[i,12]]],axis=1)
print(new_config)
reference_trajectory.append(new_config) # record the trajectory
# set commands for the youBot model in VREP
desired_world_positions[0] = new_config[0, 1]
desired_world_positions[1] = new_config[0, 2]
desired_world_positions[2] = new_config[0, 0]
desired_arm_joint_angles[0] = new_config[0, 3]
desired_arm_joint_angles[1] = new_config[0, 4]
desired_arm_joint_angles[2] = new_config[0, 5]
desired_arm_joint_angles[3] = new_config[0, 6]
desired_arm_joint_angles[4] = new_config[0, 7]
desired_wheel_positions[0] = new_config[0, 8]
desired_wheel_positions[1] = new_config[0, 9]
desired_wheel_positions[2] = new_config[0, 10]
desired_wheel_positions[3] = new_config[0, 11]
# send commands to VREP
VREP_armControl(vrep_sim, clientID, arm_joints_handle, desired_arm_joint_angles)
VREP_wheelsControl(vrep_sim, clientID, wheel_joints_handle, desired_wheel_positions)
VREP_WorldFrameControl(vrep_sim, clientID, world_joints_handle, desired_world_positions)
# set gripper state
if new_config[0, 12] == 0:
vrep_sim.simxSetJointTargetVelocity(clientID, gripper_joint_2_handle, -0.04, vrep_sim.simx_opmode_oneshot) # open
else:
vrep_sim.simxSetJointTargetVelocity(clientID, gripper_joint_2_handle, 0.04, vrep_sim.simx_opmode_oneshot) # close
return_code, gripper_joint_2_position = vrep_sim.simxGetJointPosition(clientID, gripper_joint_2_handle, vrep_sim.simx_opmode_oneshot)
vrep_sim.simxSetJointTargetPosition(clientID, gripper_joint_1_handle, -gripper_joint_2_position, vrep_sim.simx_opmode_oneshot)
time.sleep(0.002)
# Squeeze reference trajectory
reference_trajectory = np.squeeze(reference_trajectory)
# Stop VREP simulation
#print ('Stop VREP simulation')
#vrep_sim.simxStopSimulation(clientID, vrep_sim.simx_opmode_blocking)
time.sleep(0.1)
# Now close the connection to VREP
vrep_sim.simxFinish(clientID)
print?('Program?ended')#TrajectoryGenerator.py
import numpy as np
import modern_robotics as mr
def trajectoryGenerator(T_se_initial, T_sc_initial, T_sc_final, T_ce_grasp, T_ce_standoff, k):
# ---------------------------------
# segment 1: A trajectory to move the gripper from its initial configuration to a "standoff" configuration a few cm above the block
# represent the frame of end-effector when standoff in space frame
T_se_standoff_initial = T_sc_initial.dot(T_ce_standoff)
# generate trajectory when approaching the standoff position in segment 1
T_se_segment_1 = mr.CartesianTrajectory(T_se_initial,T_se_standoff_initial,5,5/0.01,3)
# ---------------------------------
# segment 2: A trajectory to move the gripper down to the grasp position
# represent the frame of end-effecto r in space frame in segment 2
T_se_grasp = T_sc_initial.dot(T_ce_grasp)
# generate trajectory when approaching the grasping position in segment 2
T_se_seg2 = mr.CartesianTrajectory(T_se_segment_1[-1], T_se_grasp, 2, 2/0.01, 3)
# append the trajectory of segment 2 after segment 1
T_se_before = np.append(T_se_segment_1, T_se_seg2, axis=0)
# ---------------------------------
# segment 3: Closing of the gripper
# append the trajectory of segment 3 by 63 times
for i in np.arange(64):
T_se_before = np.append(T_se_before,np.array([T_se_before[-1]]),axis=0)
# ---------------------------------
# segment 4: A trajectory to move the gripper back up to the "standoff" configuration
# generate trajectory when back on the standoff position in segment 4
T_se_segment_4 = mr.CartesianTrajectory(T_se_grasp, T_se_standoff_initial, 2, 2/0.01, 3)
# append the trajectory of segment 4
T_se_before = np.append(T_se_before,T_se_segment_4,axis=0)
# ---------------------------------
# segment 5: A trajectory to move the gripper to a "standoff" configuration above the final configuration
# generate trajectory when moving to the final standoff position in segment 5
T_se_standoff_final = T_sc_final.dot(T_ce_standoff)
T_se_segment_5 = mr.CartesianTrajectory(T_se_standoff_initial, T_se_standoff_final, 8, 8/0.01, 3)
# append the trajectory of segment 5
T_se_before = np.append(T_se_before, T_se_segment_5, axis=0)
# ---------------------------------
# segment 6: A trajectory to move the gripper to the final configuration of the object
# generate the end-effector configuration when losing
T_se_lose = T_sc_final.dot(T_ce_grasp)
# generate trajectory when moving to the final cube position in segment 6
T_se_segment_6 = mr.CartesianTrajectory(T_se_standoff_final, T_se_lose, 2, 2/0.01, 3)
# append the trajectory of segment 6
T_se_before = np.append(T_se_before, T_se_segment_6, axis=0)
# ---------------------------------
# segment 7: Opening of the gripper
# append the trajectory of segment 7 by 63 times
for i in np.arange(64):
T_se_before = np.append(T_se_before, np.array([T_se_before[-1]]), axis=0)
# ---------------------------------
# segment 8: A trajectory to move the gripper back to the "standoff" configuration
# generate trajectory when moving to the final standoff position in segment 8
T_se_segment_8 = mr.CartesianTrajectory(T_se_before[-1], T_se_standoff_final, 2, 2/0.01, 3)
# append the trajectory of segment 8
T_se_before = np.append(T_se_before, T_se_segment_8, axis=0)
# ---------------------------------
# generate a matrix which is n by 13
T_se_post = np.zeros([int(k*21/0.01+64*2),13])
# put the configuration, position and gripper state in matrix which is n by 13
for i in np.arange(int(k*21/0.01+64*2)):
T_se_post[i,0] = T_se_before[i,0,0]
T_se_post[i,1] = T_se_before[i,0,1]
T_se_post[i,2] = T_se_before[i,0,2]
T_se_post[i,3] = T_se_before[i,1,0]
T_se_post[i,4] = T_se_before[i,1,1]
T_se_post[i,5] = T_se_before[i,1,2]
T_se_post[i,6] = T_se_before[i,2,0]
T_se_post[i,7] = T_se_before[i,2,1]
T_se_post[i,8] = T_se_before[i,2,2]
T_se_post[i,9] = T_se_before[i,0,3]
T_se_post[i,10] = T_se_before[i,1,3]
T_se_post[i,11] = T_se_before[i,2,3]
T_se_post[i,12] = 0
# amend the gripper state in segment 3, 4, 5, 6
for i in np.arange(int(k*7/0.01), int(k*19/0.01+64)):
T_se_post[i, 12] = 1
return T_se_post#FeedbackControl.py
import numpy as np
import modern_robotics as mr
from JointLimits import jointLimits
def feedbackControl(config, Xd, Xd_next, Kp, Ki, dt, jointlimit):
# here we compute X
M = np.array([[1,0,0,0.033],[0,1,0,0],[0,0,1,0.6546],[0,0,0,1]])
Blist = np.array([[0,0,1,0,0.033,0],[0,-1,0,-0.5076,0,0],[0,-1,0,-0.3526,0,0],[0,-1,0,-0.2176,0,0],[0,0,1,0,0,0]]).T
Tb0 = np.array([[1,0,0,0.1662],[0,1,0,0],[0,0,1,0.0026],[0,0,0,1]])
thetalist_initial = np.array([config[0,3],config[0,4],config[0,5],config[0,6],config[0,7]])
T_sb_initial = np.array([[np.cos(config[0,0]),-np.sin(config[0,0]),0,config[0,1]],[np.sin(config[0,0]),np.cos(config[0,0]),0,config[0,2]],[0,0,1,0.0963],[0,0,0,1]])
X = T_sb_initial.dot(Tb0).dot(mr.FKinBody(M,Blist,thetalist_initial))
# here we write down Vd
Vd = mr.se3ToVec((1/dt)*mr.MatrixLog6(np.linalg.inv(Xd).dot(Xd_next)))
# here we write down Vb = Ad*Vd
Vb = mr.Adjoint(np.linalg.inv(X).dot(Xd)).dot(Vd)
# here we write down X_err
X_err = mr.se3ToVec(mr.MatrixLog6(np.linalg.inv(X).dot(Xd)))
# here we write down commanded twist
V = Vb+Kp*X_err+Ki*(X_err+X_err*dt)
# here we compute J_e
# first we write down J_arm
Blist = np.array([[0,0,1,0,0.033,0],[0,-1,0,-0.5076,0,0],[0,-1,0,-0.3526,0,0],[0,-1,0,-0.2176,0,0],[0,0,1,0,0,0]]).T
thetalist = np.array([config[0,3],config[0,4],config[0,5],config[0,6],config[0,7]])
J_arm = mr.JacobianBody(Blist,thetalist)
# second we write down J_base
r = 0.0475
l = 0.47/2
w = 0.3/2
gamma1 = -np.pi/4
gamma2 = np.pi/4
gamma3 = -np.pi/4
gamma4 = np.pi/4
beta = 0
x1 = l
y1 = w
x2 = l
y2 = -w
x3 = -l
y3 = -w
x4 = -l
y4 = w
# here we write down F6
a1 = np.array([1,np.tan(gamma1)])
a2 = np.array([1,np.tan(gamma2)])
a3 = np.array([1,np.tan(gamma3)])
a4 = np.array([1,np.tan(gamma4)])
b = np.array([[np.cos(beta),np.sin(beta)],[-np.sin(beta),np.cos(beta)]])
c1 = np.array([[-y1,1,0],[x1,0,1]])
c2 = np.array([[-y2,1,0],[x2,0,1]])
c3 = np.array([[-y3,1,0],[x3,0,1]])
c4 = np.array([[-y4,1,0],[x4,0,1]])
h1 = (((1/r)*a1).dot(b)).dot(c1)
h2 = (((1/r)*a2).dot(b)).dot(c2)
h3 = (((1/r)*a3).dot(b)).dot(c3)
h4 = (((1/r)*a4).dot(b)).dot(c4)
H0 = np.vstack((h1,h2,h3,h4))
F = np.linalg.pinv(H0)
F6 = np.array([[0,0,0,0],[0,0,0,0],[F[0,0],F[0,1],F[0,2],F[0,3]],[F[1,0],F[1,1],F[1,2],F[1,3]],[F[2,0],F[2,1],F[2,2],F[2,3]],[0,0,0,0]])
# then write down J_base
T_sb = np.array([[np.cos(config[0,0]),-np.sin(config[0,0]),0,config[0,1]],[np.sin(config[0,0]),np.cos(config[0,0]),0,config[0,2]],[0,0,1,0.0963],[0,0,0,1]])
T_eb = np.linalg.inv(X).dot(T_sb)
J_base = mr.Adjoint(T_eb).dot(F6)
# here we test joint limits
if jointlimit == 1:
jointLimits(config,J_arm)
# finally we write down J_e
J_e = np.hstack((J_base,J_arm))
# here we write down speeds (u,thetadot)
speeds = np.linalg.pinv(J_e,rcond=1e-2).dot(V)
return V, speeds, X_err#NextState.py
import numpy as np
def nextState(config,speed,delta_t,speed_max):
# here we limit speed
for j in np.arange(np.shape(speed)[1]):
if speed[0,j]>speed_max:
speed[0,j] = speed_max
elif speed[0,j]<-speed_max:
speed[0,j] = -speed_max
# here we get the new arm and wheels configurations
new_angle_config = config[0,3:12]+speed*delta_t
# here we start to compute the configuration of chassis using odometry
# step1: compute delta theta, which is the change of wheels rotation
delta_theta = (speed[0,5:].reshape(1,4)).T*delta_t
# step2: compute the wheels velocities
theta_dot = delta_theta
# step3: compute chassis planar twist Vb
# here we write down some necessary parameters for step3
r = 0.0475
l = 0.47/2
w = 0.3/2
Vb = (r/4)*np.array([[-1/(l+w),1/(l+w),1/(l+w),-1/(l+w)],[1,1,1,1],[-1,1,-1,1]]).dot(theta_dot)
# step4: calculate new chassis config
# here we write down dqb
if Vb[0,0] == 0:
dqb = np.array([[0],[Vb[1,0]],[Vb[2,0]]])
elif Vb[0,0] != 0:
dqb = np.array([[Vb[0,0]],[(Vb[1,0]*np.sin(Vb[0,0])+Vb[2,0]*(np.cos(Vb[0,0])-1))/Vb[0,0]],[(Vb[2,0]*np.sin(Vb[0,0])+Vb[1,0]*(1-np.cos(Vb[0,0])))/Vb[0,0]]])
# here we write down dq
dq = np.array([[1,0,0],[0,np.cos(config[0,0]),-np.sin(config[0,0])],[0,np.sin(config[0,0]),np.cos(config[0,0])]]).dot(dqb)
# here we write down new chassis config
new_chassis_config = config[0,0:3].reshape(1,3)+dq.reshape(1,3)
# here we put the angle and chassis configuration together
new_config = np.hstack((new_chassis_config,new_angle_config))
????return?new_config#jointLimits.py
import numpy as np
def jointLimits(config,J_arm):
if config[0,5] > -0.1:
J_arm[:,2] = np.array([0,0,0,0,0,0])
if config[0,6] > -0.1:
J_arm[:,3] = np.array([0,0,0,0,0,0])
注释截图
MR API笔记
笛卡尔轨迹生成：
traj =CartesianTrajectory(Xstart,Xend,Tf,N,method)
输入：
Xstart: 初始末端执行器配置Xstart ∈ SE(3).
Xend: 最终末端执行器配置Xend.
Tf: 从静止到静止的运动总时间 Tf（以秒为单位）。
N: 轨迹离散表示中的点数 N≥2。
method：时间标度方法，其中 3 表示三次（三阶多项式）时间标度
5 表示五次（五阶多项式）时间标度。
输出：
traj: 离散化的轨迹，作为SE(3) 中 N 个矩阵的列表，在时间上由 Tf /(N-1) 分隔。列表中的第一个是 Xstart，第 N 个是 Xend。
正向运动学（物体坐标系）：
T = FKinBody(M,Blist,thetalist)
输入：
M: 末端执行器的home配置
Blist: The joint screw axes in theend-effector frame when the manipulator is at the home position. 机械手在home时末端执行器坐标系中的关节螺旋轴
thetalist: 关节坐标值列表。
输出：
T: The T ∈ SE(3) representing theend-effector frame when the joints are at the specified coordinates. T ∈ SE(3) 表示关节在指定坐标值时的末端执行器坐标系位姿。
?
se3mat = MatrixLog6(T)
输入：
T: Transformation matrix. 变换矩阵
输出：
se3mat: The corresponding se(3)representation of exponential coordinates. 指数坐标的相应se(3) 表示。
AdT = Adjoint(T)
输入：
T：变换矩阵
输出：
AdT: The corresponding 6 × 6 adjointrepresentation [AdT ].
V = se3ToVec(se3mat)
输入：
se3mat: A 4 × 4 se(3) matrix.
输出：
V: The corresponding 6-vecto
控制律：
备注：其他公式参考书中第十三章内容。
参考：
https://blog.csdn.net/shakehands2012/article/details/109233371?spm=1001.2014.3001.5501
https://github.com/chauby/V-REP-YouBot-Demo