def c_planner(dt, traj, oMi, oMi_init, t_simu):
eigenpy.switchToNumpyMatrix()
EPS = 1e-6
# Update Target oMi position
if np.linalg.norm(t_simu - 0.0) < EPS:
oMi_ref = copy.deepcopy(oMi_init)
oMi_ref.translation = oMi_ref.translation + np.matrix(
[0.0, 0.05, 0.05]).transpose()
traj.setInitialPosture(oMi_init)
traj.setFinalPosture(oMi_ref)
traj.setDuration(1.0)
traj.setStartTime(0.0)
elif np.linalg.norm(t_simu - 1.0) < EPS:
oMi_ref = copy.deepcopy(oMi_init)
oMi_ref.translation = oMi_ref.translation + np.matrix([0.0, 0.0, 0.1
]).transpose()
traj.setInitialPosture(oMi)
traj.setFinalPosture(oMi_ref)
traj.setDuration(1.0)
traj.setStartTime(1.0)
elif np.linalg.norm(t_simu - 2.0) < EPS:
oMi_ref = copy.deepcopy(oMi_init)
traj.setInitialPosture(oMi)
traj.setFinalPosture(oMi_ref)
traj.setDuration(1.0)
traj.setStartTime(2.0)
sample = traj.computeNext(t_simu)
return sample
def __init__(self):
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group_name = "manipulator"
self.group = moveit_commander.MoveGroupCommander(self.group_name)
self.compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
eigenpy.switchToNumpyMatrix()
def main():
eigenpy.switchToNumpyMatrix()
rospy.init_node('planner')
while not rospy.get_time() == 0: #wait until the system gets the time
continue
rospy.wait_for_service('/compute_ik')
'''
#get a end effector pose to test the ik
rospy.wait_for_service('/gazebo/get_link_state')
get_link_state = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
link_state_request=GetLinkStateRequest()
link_state_request.link_name='wrist_3_link'
#link_state_request.reference_frame='base_link'
link_state_reponse=get_link_state(link_state_request)
end_effector_pose=link_state_reponse.link_state.pose
#print(end_effector_pose)
#end_effector_pose.position.z-=0.1
#end_effector_pose.position.x-=0.2
'''
current_planner = planner()
rate = rospy.Rate(10)
start_time = rospy.get_time()
with open(
'/home/chen/ws_chen/src/mm_meta_pkg/mobile_manipulator/data/base_trajectory_area.txt',
'w') as f:
f.write('base trajectory: x y theta time \r\n')
#current_planner.plan_whole_trajectory_max_mani()
current_planner.plan_whole_trajectory_biggest_area()
def c_control(q, qdot, dt, robot, t_simu):
eigenpy.switchToNumpyMatrix()
# Get dynamics information
M = robot.mass(q) # Mass Matrix
NLE = robot.nle(q, qdot) #gravity and Coriolis
# Torque from Non linear compensation term
torque = NLE
# For our controller will be here
return torque[6:]
def c_control(dt, robot, sample, t_simu):
eigenpy.switchToNumpyMatrix()
qa = robot.q # actuation joint position
qa_dot = robot.v # actuation joint velocity
# Method : Joint Control A q_ddot = torque
Kp = 400. # Convergence gain
ID_EE = robot.model.getFrameId(
"LWrMot3") # Control target (End effector) ID
# Compute/update all joints and frames
se3.computeAllTerms(robot.model, robot.data, qa, qa_dot)
# Get kinematics information
oMi = robot.framePlacement(qa, ID_EE) ## EE's placement
v_frame = robot.frameVelocity(qa, qa_dot, ID_EE) # EE's velocity
J = robot.computeFrameJacobian(qa,
ID_EE) ## EE's Jacobian w.r.t Local Frame
# Get dynamics information
M = robot.mass(qa) # Mass Matrix
NLE = robot.nle(qa, qa_dot) #gravity and Coriolis
Lam = np.linalg.inv(J * np.linalg.inv(M) * J.transpose()) # Lambda Matrix
# Update Target oMi position
wMl = copy.deepcopy(sample.pos)
wMl.rotation = copy.deepcopy(oMi.rotation)
v_frame_ref = se3.Motion() # target velocity (v, w)
v_frame_ref.setZero()
# Get placement Error
p_error = se3.log(sample.pos.inverse() * oMi)
v_error = v_frame - wMl.actInv(v_frame_ref)
# Task Force
p_vec = p_error.vector
v_vec = v_error.vector
for i in range(0, 3): # for position only control
p_vec[i + 3] = 0.0
v_vec[i + 3] = 0.0
f_star = Lam * (-Kp * p_vec - 2.0 * np.sqrt(Kp) * v_vec)
# Torque from Joint error
torque = J.transpose() * f_star + NLE
torque[0] = 0.0
return torque
def main(move_type, write_type):
eigenpy.switchToNumpyMatrix()
rospy.wait_for_service('/compute_fk')
while rospy.get_time() == 0: #wait until the rospy gets the system time
continue
manipulator = manipulator_state_class(move_type, write_type)
rate = rospy.Rate(10)
rospy.loginfo('the manipulator_state node is working!!')
if manipulator.move_type == 0 and manipulator.write_type == 1:
with open(
'/home/chen/ws_chen/src/mm_meta_pkg/manipulator_moveit_config/data/manipulator_state_desired.txt',
'w') as f:
f.write(
'desired robot state:time position_x position_y position_z R P Y \r\n '
)
if manipulator.move_type == 1 and manipulator.write_type == 1:
with open(
'/home/chen/ws_chen/src/mm_meta_pkg/manipulator_moveit_config/data/manipulator_state_noise.txt',
'w') as f:
f.write(
'noise_added robot state:time position_x position_y position_z R P Y\r\n '
)
if manipulator.move_type == 2 and manipulator.write_type == 1:
with open(
'/home/chen/ws_chen/src/mm_meta_pkg/manipulator_moveit_config/data/manipulator_state_controlled.txt',
'w') as f:
f.write(
'controlled robot state:time position_x position_y position_z R P Y\r\n '
)
while not rospy.is_shutdown():
'''
pose=PoseStamped()
pose.pose.orientation.x=0
pose.pose.orientation.y=0
pose.pose.orientation.z=0
pose.pose.orientation.w=0.866
manipulator.quaternion_to_euler(pose.pose.orientation)
'''
rate.sleep()
import numpy as np
# Pinocchio modules
import pinocchio as se3 # Pinocchio library
import eigenpy
eigenpy.switchToNumpyMatrix()
import copy
################
# PlANNERL ##
################
def c_planner(dt, traj, oMi, oMi_init, t_simu):
eigenpy.switchToNumpyMatrix()
EPS = 1e-6
# Update Target oMi position
if np.linalg.norm(t_simu - 0.0) < EPS:
oMi_ref = copy.deepcopy(oMi_init)
oMi_ref.translation = oMi_ref.translation + np.matrix(
[0.0, 0.05, 0.05]).transpose()
traj.setInitialPosture(oMi_init)
traj.setFinalPosture(oMi_ref)
traj.setDuration(1.0)
traj.setStartTime(0.0)
elif np.linalg.norm(t_simu - 1.0) < EPS:
oMi_ref = copy.deepcopy(oMi_init)
oMi_ref.translation = oMi_ref.translation + np.matrix([0.0, 0.0, 0.1
]).transpose()
def c_control(dt, robot, sample, t_simu):
eigenpy.switchToNumpyMatrix()
qa = np.matrix(np.zeros((7, 1)))
qa_dot = np.matrix(np.zeros((7, 1)))
for i in range(7):
qa[i, 0] = robot.q[i]
qa_dot[i, 0] = robot.v[i]
# Method : Joint Control A q_ddot = torque
Kp = 400. # Convergence gain
ID_EE = robot.model.getFrameId(
"LWrMot3") # Control target (End effector) ID
# Compute/update all joints and frames
se3.computeAllTerms(robot.model, robot.data, qa, qa_dot)
# Get kinematics information
oMi = robot.framePlacement(qa, ID_EE) ## EE's placement
v_frame = robot.frameVelocity(qa, qa_dot, ID_EE) # EE's velocity
J = robot.computeFrameJacobian(
qa, ID_EE)[:3, :] ## EE's Jacobian w.r.t Local Frame
# Get dynamics information
M = robot.mass(qa) # Mass Matrix
NLE = robot.nle(qa, qa_dot) #gravity and Coriolis
Lam = np.linalg.inv(J * np.linalg.inv(M) * J.transpose()) # Lambda Matrix
# Update Target oMi position
wMl = copy.deepcopy(sample.pos)
wMl.rotation = copy.deepcopy(oMi.rotation)
v_frame_ref = se3.Motion() # target velocity (v, w)
v_frame_ref.setZero()
# Get placement Error
p_error = se3.log(sample.pos.inverse() * oMi)
v_error = v_frame - wMl.actInv(v_frame_ref)
# Task Force
p_vec = p_error.linear
v_vec = v_error.linear
xddotstar = (-Kp * p_vec - 2 * np.sqrt(Kp) * v_vec)
f_star = Lam * xddotstar
# Torque from Joint error
torque = J.transpose() * f_star + NLE
torque[0] = 0.0
# Selection Matrix
Id7 = np.identity(7)
fault_joint_num = 0
S = np.delete(Id7, (fault_joint_num),
axis=0) #delete fault joint corresponding row
# Selection Matrix Inverse - dynamically consistent inverse
Minv = np.linalg.inv(M)
ST = S.transpose()
SbarT = np.linalg.pinv(S * Minv * ST) * S * Minv
# Jacobian Matrix Inverse - dynamically consistent inverse
JbarT = Lam * J * np.linalg.inv(M)
#Weighting matrix
W = S * Minv * ST
Winv = np.linalg.inv(W)
#Weighted pseudo inverse of JbarT*ST
JtildeT = Winv * (JbarT * ST).transpose() * np.linalg.pinv(
JbarT * ST * Winv * (JbarT * ST).transpose())
#Null-space projection matrix
Id6 = np.identity(6)
NtildeT = Id6 - JtildeT * JbarT * ST
null_torque = 0.0 * qa_dot # joint damping
#Torque
torque = ST * (JtildeT * f_star + NtildeT * SbarT * null_torque +
SbarT * NLE)
return torque
quat = eigenpy.Quaternion()
a = [0., 0., 0.]
cmd1 = "timeit np.array(a)"
print("\n")
print(cmd1)
ipython.magic(cmd1)
print("\n")
cmd2 = "timeit np.matrix(a)"
print(cmd2)
ipython.magic(cmd2)
print("\n")
eigenpy.switchToNumpyMatrix()
print("----------------------")
print("switch to numpy matrix")
print("----------------------")
print("\n")
cmd3 = "timeit quat.coeffs()"
print(cmd3)
ipython.magic(cmd3)
print("\n")
eigenpy.switchToNumpyArray()
print("---------------------")
print("switch to numpy array")
print("---------------------")
print("\n")
def main(move_type):
eigenpy.switchToNumpyMatrix()
cmd_pub = rospy.Publisher('/manipulator_controller/command',
JointTrajectory,
queue_size=10)
rospy.loginfo("starting to publish commands")
'''
#using moveit to control the arm
manipulator_control=manipulator_control_class()
rate=rospy.Rate(10)
manipulator_control.go_to_goal_joint(math.pi/2,0,0,0,0,0)
manipulator_control.go_to_home_joint()
'''
'''
# controller type: velocity_controllers/JointVelocityController
while not rospy.is_shutdown():
command=Float64MultiArray()
command.data=[0,0,0,0,0,0]
if rospy.get_time()-start_time < 5:
command.data[2]=2
elif rospy.get_time()-start_time < 10:
command.data[2]=-2
else:
exit()
cmd_pub.publish(command)
rate.sleep()
'''
#controller type : velocity_controllers/JointTrajectoryController
rospy.wait_for_service('/compute_fk')
manipulator_control = manipulator_control_class()
manipulator_control.start_to_sub = True
while rospy.get_time() == 0: #wait until the rospy gets the system time
continue
manipulator_control.start_time = rospy.get_time()
is_first = True # to help test the time
if move_type == 'desired':
manipulator_control.move_type = 1
elif move_type == 'noise_added':
manipulator_control.move_type = 2
elif move_type == 'controlled':
manipulator_control.move_type = 3
else:
rospy.logerr(
'invalid arg!plz input desired or noise_added or controlled')
exit()
manipulator_control.is_control = True #false means the command will not be published
if manipulator_control.is_control == False:
rospy.loginfo('the trajectory command will not be published')
rate = rospy.Rate(manipulator_control.rate_hz)
latter_joint_value = [0, -0.3, -2, 0, 1,
0] #init joint values of the manipulator
latter_joint_velocity = np.zeros(
(6, 1)) #init joint velocity of the manipulator
while not rospy.is_shutdown():
if manipulator_control.got_msg == True:
time = rospy.get_time() - manipulator_control.start_time
if is_first == True and time > 2:
rospy.logerr(
'something wrong with the timer, plz restart the node')
exit()
is_first = False
command = JointTrajectory()
command.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
point = JointTrajectoryPoint()
desired_cartisian_velocity, desired_joint_velocity, desired_joint_value = manipulator_control.compute_desired_velocity(
time, latter_joint_value, latter_joint_velocity)
latter_joint_value = copy.deepcopy(desired_joint_value)
latter_joint_velocity = copy.deepcopy(desired_joint_velocity)
#move type of the manipulator
if manipulator_control.move_type == 1: # the desired trajectory
joint_velocity_command = copy.deepcopy(desired_joint_velocity)
elif manipulator_control.move_type == 2: # the desired trajectory with velocity noise
joint_velocity_command = manipulator_control.add_noise(
desired_joint_velocity, time)
elif manipulator_control.move_type == 3: # the controlled trajectory with velocity noise
desired_cartisian_pose = manipulator_control.compute_f_k(
manipulator_control.compute_fk, desired_joint_value)
#this desired_cartisian_pose is a PoseStamped
controlled_joint_velocity = manipulator_control.controlled_command(
desired_cartisian_velocity, desired_cartisian_pose,
manipulator_control.current_end_effector_pose)
joint_velocity_command = manipulator_control.add_noise(
controlled_joint_velocity, time)
#publishs the joint velocity command
for i in range(6):
point.positions.append(
manipulator_control.current_state.desired.positions[i] +
joint_velocity_command[i, 0])
point.velocities.append(joint_velocity_command[i, 0])
point.time_from_start = rospy.Duration(1.0)
command.points.append(point)
command.header.seq = command.header.seq + 1
command.header.stamp = rospy.Time.now()
if manipulator_control.is_control == True:
cmd_pub.publish(command)
rate.sleep()
