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