def __init__(self): rospy.init_node('UR5_motion_planner', anonymous=True) self.ros_rate = rospy.Rate(self.freq) self.move_arm_action_server = actionlib.SimpleActionClient('follow_joint_trajectory/goal', FollowJointTrajectoryAction) self.move_arm_pub_gazebo = rospy.Publisher('arm_controller/command', JointTrajectory, queue_size=20, latch=True) self.joint_name = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] self.kin = Kinematics("ur5") self.single_sol = False
def test_q(q): global kin kin = Kinematics('ur10') x = kin.forward(q) sols = kin.inverse(np.array(x), q, test=True) qsol = best_sol(sols, q, [1.] * 6) if qsol is None: qsol = [999.] * 6 diff = np.sum(np.abs(np.array(qsol) - q)) if diff > 0.001: print np.array(sols) print 'Best q:', qsol print 'Actual:', np.array(q) print 'Diff: ', q - qsol print 'Difdiv:', (q - qsol) / np.pi #print i1-3, i2-3, i3-3, i4-3, i5-3, i6-3 if raw_input() == 'q': sys.exit()
def main(): global MODE_DEBUG, MODE_REDUCE_FULL try: if (len(sys.argv) > 1): if (sys.argv[1] == 'debug'): MODE_DEBUG = True #print "MODE_DEBUG=" + str(MODE_DEBUG) if (len(sys.argv) > 2): if (sys.argv[2] == 'full'): MODE_REDUCE_FULL = False #print "Mode Debug Full ? : " + str(MODE_REDUCE_FULL) c_print(str("Instanciation Kinematic UR10")) kin = Kinematics('ur10') inp = raw_input("Run Script Test ? Y/N: ")[0] if (inp == 'Y' or inp == 'y'): np.set_printoptions(precision=3) print("Testing multiples of pi/2...") for i1 in range(0, 5): for i2 in range(0, 5): c_print(str(i1) + str(i2), MODE_REDUCE_FULL) for i3 in range(0, 5): for i4 in range(0, 5): for i5 in range(0, 5): for i6 in range(0, 5): q = np.array([ i1 * np.pi / 2., i2 * np.pi / 2., i3 * np.pi / 2., i4 * np.pi / 2., i5 * np.pi / 2., i6 * np.pi / 2. ]) test_q(q) exit print "Testing random configurations..." for i in range(10000): q = (np.random.rand(6) - .5) * 4 * np.pi test_q(q) print "Done!" else: print "Halting program" except KeyboardInterrupt: rospy.signal_shutdown("KeyboardInterrupt") raise
class Ur5_motion_planner(object): joint_lim_low = [-1,-0.5,-0.5,-np.pi,-np.pi,-np.pi] # Default joint limits. joint_lim_high = [-i for i in joint_lim_low] freq = 275 # Hz max_angular_vel = 2.5 # rad /s min_angular_vel = 1.5 def __init__(self): rospy.init_node('UR5_motion_planner', anonymous=True) self.ros_rate = rospy.Rate(self.freq) self.move_arm_action_server = actionlib.SimpleActionClient('follow_joint_trajectory/goal', FollowJointTrajectoryAction) self.move_arm_pub_gazebo = rospy.Publisher('arm_controller/command', JointTrajectory, queue_size=20, latch=True) self.joint_name = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] self.kin = Kinematics("ur5") self.single_sol = False # super(Ur5_motion_planner, self).__init__(*args, **kwar gs) def __unicode__(self): return """UR5 custom motion planning library""" @classmethod def change_joint_limits(cls,lim_low,lim_high): try: cls.__verify_joint_input(lim_low,lim_high) cls.joint_lim_low = lim_low cls.joint_lim_high = lim_high except AssertionError as e: print "Wrong input length or type for limits" except Exception as e: # Do not interrupt programme flow by breaking code print e return @staticmethod def __verify_joint_input(lim_low,lim_high): assert type(lim_low) is list and len(lim_low) == 6 assert type(lim_high) is list and len(lim_high) == 6 for i in range(6): if lim_low[i] > lim_high[i]: raise Exception('Please sort your high and low limits') return def cartesian_to_ik(self, cartesian, single_sol=False): # Cartesian coordinates of [roll,pitch,yaw,X,Y,Z] to ik solutions # Returns all possible IK solutions within joint limits if single is False # Returns best IK solution within joint limit if single is True self.single_sol = single_sol roll,pitch,yaw,X,Y,Z = cartesian matrix_from_euler = TF.euler_matrix(roll,pitch,yaw) matrix_from_translation = TF.translation_matrix([X,Y,Z]) - TF.identity_matrix() matrix_from_cartesian = matrix_from_euler + matrix_from_translation return self.__get_ik_from_matrix(matrix_from_cartesian) def __get_ik_from_matrix(self, h_matrix): # Gets all possible IK solutions from homogeneous matrix that satisfies joint limits defined # If no possible IK solutions for given point, throws AssertionError ik_sols = self.kin.inverse_all(x=h_matrix) ik_sols_cleaned = self.__assert_joint_limits(ik_sols) assert len(ik_sols_cleaned) is not 0 if(self.single_sol): ik_sols_cleaned = self.__get_best_ik_solution(ik_sols_cleaned) return ik_sols_cleaned def __assert_joint_limits(self, ik_sols): # Asserts that the given joint space coordinate falls within the allowable joint limits or throw AssertionError ik_sols_cleaned = [] for each_ik_sol in ik_sols: if self.__solution_within_joint_limits(each_ik_sol): ik_sols_cleaned += each_ik_sol return ik_sols_cleaned def __get_best_ik_solution(self, ik_sols_cleaned, weights=6*[1.0]): q_guess = np.zeros(6) import ipdb ipdb.set_trace() best_sol_ind = np.argmin(np.sum((weights*(ik_sols_cleaned - np.array(q_guess)))**2,1)) best_sol = ik_sols_cleaned[best_sol_ind] return best_sol def __solution_within_joint_limits(self, each_ik_sol): # If not within joint limit, reject each_ik_sol length = len(each_ik_sol) for i in range(length): # Check if joint limit has been exceed for each UR joint if each_ik_sol[i] < self.joint_lim_low[i] or each_ik_sol[i] > self.joint_lim_high[i]: return False return True def cartesian_from_joint(self, joint_vals): # Gets homogeneous matrix from joint values # joint_vals either be a list of multiple joint sets or a single joint set if(all(isinstance(i, list) for i in joint_vals)): lst = [] for each_joint_val in joint_vals: fk_sol = self.kin.forward(q=each_joint_val) lst += [self.__get_cartesian_from_matrix(fk_sol)] else: fk_sol = self.kin.forward(q=joint_vals) lst = self.__get_cartesian_from_matrix(fk_sol) return lst def __get_cartesian_from_matrix(self, h_matrix): # Returns a list of [roll, pitch, yaw, X, Y, Z] euler_from_matrix = list(TF.euler_from_matrix(h_matrix)) translation_from_matrix = list(TF.translation_from_matrix(h_matrix)) return euler_from_matrix + translation_from_matrix def plan_to_cartesian(self, current_joint_val, goal_cartesian, no_points=10): """ Plans to given cartesian from current coordinate with intermediate way points. """ # Goal cartesian is a list of [roll,pitch,yaw,X,Y,Z] # Start with a linear planner that plans in cartesian space using SLERP interpolation # Raises AssertinError when intermediate point has no solutions current_cartesian = self.cartesian_from_joint(current_joint_val) joint_way_points = [] try: for i in list(reversed(range(no_points))): point = linear_pose_interp(current_cartesian, goal_cartesian, (i+1.0) /no_points) print point way_point = quat2euler(point['rot']) + point['lin'] import ipdb ipdb.set_trace() joint_way_points += self.cartesian_to_ik(cartesian=way_point, single_sol=True) except AssertionError: raise AssertionError("No possible IK solutions found for coordinate: {0}".format(way_point)) return joint_way_points def _frame_message(self, joint_space, time_interp): interpolation_time = time_interp # seconds joint_traj_msg = JointTrajectory() sensor_msg_header = Header() JointTrajectoryPoint_points = JointTrajectoryPoint() sensor_msg_header.seq = 1 sensor_msg_header.stamp = rospy.get_rostime() sensor_msg_header.frame_id = "10" positions = [-i for i in joint_space] positions[5] *= -1 # Some hacks to make Gazebo model the same as OpenRave JointTrajectoryPoint_points.positions = positions JointTrajectoryPoint_points.velocities = [] JointTrajectoryPoint_points.accelerations = [] JointTrajectoryPoint_points.effort = [] JointTrajectoryPoint_points.time_from_start = rospy.Duration(interpolation_time) joint_traj_msg.header = sensor_msg_header joint_traj_msg.joint_names = self.joint_names joint_traj_msg.points = [JointTrajectoryPoint_points] return joint_traj_msg def __publish_joint_msg(self, single_joint_space, time_interp=2): joint_traj_msg = self._frame_message(single_joint_space, time_interp) self.move_arm_pub_gazebo.publish(joint_traj_msg) self.ros_rate.sleep() # rospy.sleep(1) def __move_arm_single(self, joint_space): self.__publish_joint_msg(single_joint_space=joint_space) def __get_velocity_ramp(self, itr): max_no_points = self.max_no_points if itr <= max_no_points /3: vel = (3 *float(itr) /max_no_points) *(self.max_angular_vel -self.min_angular_vel) + self.min_angular_vel elif itr >= max_no_points *2/3: vel = self.max_angular_vel -(3 *float(itr) /max_no_points -2) * (self.max_angular_vel -self.min_angular_vel) else: vel = self.max_angular_vel return vel def __generate_ramp_trajectory(self, joint_space): itr = 0 self.max_no_points = joint_space['length'] for each_trajectory in joint_space['trajectory']: target = each_trajectory if(itr): max_angle_change = max(abs(target -start)) velocity = self.__get_velocity_ramp(itr) time_interp = float(max_angle_change / velocity) self.__publish_joint_msg(single_joint_space=target, time_interp=time_interp) start = target itr += 1 def __move_arm_trajectory(self, joint_space, v_profile): # Moves the arm according to an np.array of joint trajectories if v_profile == "ramp": # Ramp velocity specified self.__generate_ramp_trajectory(joint_space) else: rospy.logerr("Move arm trajectory -> Invalid v_profile selected") raise Exception() def move_arm(self, joint_space, v_profile=None): if(v_profile != None): self.__move_arm_trajectory(joint_space, v_profile) else: self.__move_arm_single(joint_space)
#!/usr/bin/env python import time import roslib import rospy import actionlib import numpy as np import matplotlib.pyplot as plt from geometry_msgs.msg import WrenchStamped from control_msgs.msg import * from trajectory_msgs.msg import * from sensor_msgs.msg import JointState from math import pi from ur_kin_py.kin import Kinematics kin = Kinematics('ur5') # or ur10 JOINT_NAMES = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] Q = [0, -1.5, 0, 0, 0, 0] ####[0.7527102129775596, -0.8800007909400067, 1.4077562405750825, 0.12729927026862775, 1.4077552689732853, -0.0001616771659893601] ####[0.822754102249533, -0.8845703281457675, 1.4392645087525198, 0.06182310686880754, 1.439264241579239, 0.0007715636829370709] ####[0.8259268065028502, -0.8866035137180859, 1.4391278111774177, 0.06068517804702722, 1.4391276071598087, -1.9205572225899914e-05] ####[0.8124250759361349, -0.8856928258437353, 1.3718855603493418, 0.07327620054303008, 1.3718850169411079, -0.00015531381389610743] QQ = [ 1.39793490e+00, -8.88942791e-01, 8.49105951e-01, 3.98530081e-02, 1.39793228e+00, 1.47085210e-06 ] #[1.40837903e+00, -8.89101630e-01, 8.56433561e-01, 3.26842802e-02, 1.40837641e+00, -2.16795492e-06]
class MarioKinematics(object): joint_lim_low = [ -2 * np.pi, -2 * np.pi, -2 * np.pi, -2 * np.pi, -2 * np.pi, -2 * np.pi ] # Default joint limits. joint_lim_high = [-i for i in joint_lim_low] kin = Kinematics("ur5") def __init__(self, is_simulation, *args, **kwargs): # rospy.init_node('UR5_motion_planner', anonymous=True) # self.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] self.__robot_joint_state = [] if (is_simulation): rospy.Subscriber('/arm_controller/state', JointTrajectoryControllerState, self.__robot_joint_state_callback_simulation) else: rospy.Subscriber('/joint_states', JointState, self.__robot_joint_state_callback_actual) super(MarioKinematics, self).__init__(is_simulation, *args, **kwargs) def __unicode__(self): return """UR5 custom motion planning library""" def __robot_joint_state_callback_actual(self, data): self.__robot_joint_state = data.position return def __robot_joint_state_callback_simulation(self, data): actual_joint_state_msg_frame = data.actual self.__robot_joint_state = actual_joint_state_msg_frame.positions return @classmethod def change_joint_limits(cls, lim_low, lim_high): try: cls.__verify_joint_input(lim_low, lim_high) cls.joint_lim_low = lim_low cls.joint_lim_high = lim_high except AssertionError as e: print "Wrong input length or type for limits" except Exception as e: # Do not interrupt programme flow by breaking code print e return @classmethod def __verify_joint_input(cls, lim_low, lim_high): assert type(lim_low) is list and len(lim_low) == 6 assert type(lim_high) is list and len(lim_high) == 6 for i in range(6): if lim_low[i] > lim_high[i]: raise Exception('Please sort your high and low limits') return @classmethod def cartesian_from_joint(cls, joint_vals): # Returns a list of [roll, pitch, yaw, X, Y, Z] from joint values # joint_vals either be a list of multiple joint sets or a single joint set if (all(isinstance(i, list) for i in joint_vals)): lst = [] for each_joint_val in joint_vals: fk_sol = cls.kin.forward(q=each_joint_val) lst += [cls.__get_cartesian_from_matrix(fk_sol)] else: fk_sol = cls.kin.forward(q=joint_vals) lst = cls.__get_cartesian_from_matrix(fk_sol) return lst @classmethod def __get_cartesian_from_matrix(cls, h_matrix): # Returns a list of [roll, pitch, yaw, X, Y, Z] euler_from_matrix = list(TF.euler_from_matrix(h_matrix)) translation_from_matrix = list(TF.translation_from_matrix(h_matrix)) return euler_from_matrix + translation_from_matrix def get_joint_sol_from_bin_grasping(self, obj_label, grasp_results, grasp_type): # obj_label is the identifier for bin or tote location suction_method = self.__get_gripper_suction_method(grasp_type) roll, pitch, yaw, item_coord_X, item_coord_Y, item_coord_Z = grasp_results # roll = -pi/2 # pitch = 0 # yaw = 0 # item_coord_X, item_coord_Y, item_coord_Z = grasp_results gripper_coord_X, gripper_coord_Y, gripper_coord_Z, _ = RobotToNewShelfTransformation.get_item_coord_from_obj_to_robot( obj_label, item_coord_X, item_coord_Y, item_coord_Z) gripper_offset_X, gripper_offset_Y, gripper_offset_Z = suction_method.gripper_frame_to_end_effector_displacement( roll, pitch, yaw) base_coord_X = gripper_coord_X - gripper_offset_X base_coord_Y = gripper_coord_Y - gripper_offset_Y base_coord_Z = gripper_coord_Z - gripper_offset_Z cartesian = [ roll, pitch, yaw, base_coord_X, base_coord_Y, base_coord_Z ] candidate_sols = self.cartesian_to_ik(cartesian=cartesian) selected_ik_sol = self.kin.get_closest_joint_sol( current_joint=self.get_robot_joint_state(), candidate_sols=candidate_sols) selected_ik_sol[0:5] *= -1 # HACKS FOR GAZEBO return selected_ik_sol def get_joint_sol_from_tote_grasping(self, obj_label, grasp_results, grasp_type): # obj_label is the identifier for bin or tote location suction_method = self.__get_gripper_suction_method(grasp_type) roll, pitch, yaw, item_coord_X, item_coord_Y, item_coord_Z = grasp_results base_coord_X, base_coord_Y, base_coord_Z, _ = RobotToToteTransformation.get_item_coord_from_obj_to_robot( obj_label, item_coord_X, item_coord_Y, item_coord_Z) gripper_offset_X, gripper_offset_Y, gripper_offset_Z = suction_method.gripper_frame_to_end_effector_displacement( roll, pitch, yaw) base_coord_X = gripper_coord_X - gripper_offset_X base_coord_Y = gripper_coord_Y - gripper_offset_Y base_coord_Z = gripper_coord_Z - gripper_offset_Z cartesian = [ roll, pitch, yaw, base_coord_X, base_coord_Y, base_coord_Z ] return self.cartesian_to_ik(cartesian=cartesian) def __get_gripper_suction_method(self, grasp_type): # 1 - Front suction method. 0 - Side suction method. if (grasp_type): return GripperFrontSuctionOffset else: return GripperSideSuctionOffset def cartesian_to_ik(self, cartesian): # Cartesian coordinates of [roll,pitch,yaw,X,Y,Z] to ik solutions # Returns all possible IK solutions within joint limits if single is False # Returns best IK solution within joint limit if single is True roll, pitch, yaw, X, Y, Z = cartesian matrix_from_euler = TF.euler_matrix(roll, pitch, yaw) matrix_from_translation = TF.translation_matrix([X, Y, Z]) matrix_from_cartesian = TF.concatenate_matrices( matrix_from_translation, matrix_from_euler) return self.__get_ik_from_matrix(matrix_from_cartesian) def __get_ik_from_matrix(self, h_matrix): # Gets all possible IK solutions from homogeneous matrix that satisfies joint limits defined # If no possible IK solutions for given point, throws AssertionError ik_sols = self.kin.inverse_all(x=h_matrix) ik_sols_cleaned = self.__assert_joint_limits(ik_sols) assert len(ik_sols_cleaned) is not 0 return ik_sols_cleaned def __assert_joint_limits(self, ik_sols): # Asserts that the given joint space coordinate falls within the allowable joint limits or throw AssertionError ik_sols_cleaned = [] for each_ik_sol in ik_sols: if self.__solution_within_joint_limits(each_ik_sol): ik_sols_cleaned.append(each_ik_sol) return ik_sols_cleaned def __solution_within_joint_limits(self, each_ik_sol): # If not within joint limit, reject each_ik_sol length = len(each_ik_sol) for i in range(length): # Check if joint limit has been exceed for each UR joint if each_ik_sol[i] < self.joint_lim_low[i] or each_ik_sol[ i] > self.joint_lim_high[i]: return False return True def get_joint_space_from_delta_robot_frame(self, axis, delta_dist): def get_goal_cartesian_from_delta_dist(current_cartesian, axis, delta_dist): roll, pitch, yaw, x, y, z = current_cartesian default_roll_offser = -pi / 2 # offset hardcoded to make the gripper's axis align with gazebo's axis roll -= default_roll_offser matrix_from_euler = TF.euler_matrix(roll, pitch, yaw) axis_translation_list = get_axis_translation_list( axis=axis, delta_dist=delta_dist) delta_matrix = TF.translation_matrix(axis_translation_list) delta_goal_matrix_base_frame = np.dot(matrix_from_euler, delta_matrix) delta_goal_translation_base_frame = TF.translation_from_matrix( delta_goal_matrix_base_frame) final_goal_base_frame = delta_goal_translation_base_frame + [ x, y, z ] return [roll, pitch, yaw] + final_goal_base_frame.tolist() def get_axis_translation_list(axis, delta_dist): # Only move along the axis of the robot frame if axis == 'x': axis_translation_list = [delta_dist, 0, 0] elif axis == 'y': axis_translation_list = [0, delta_dist, 0] elif axis == 'z': axis_translation_list = [0, 0, delta_dist] else: raise Exception( 'MarioKinematics -> get_axis_translation_list -> axis given is not x,y,z' ) return axis_translation_list def plan_joint_way_points(current_cartesian, goal_cartesian): way_points = PoseInterpolator.plan_to_cartesian_linear( current_cartesian, goal_cartesian) joint_way_points = [] current_joint = self.get_robot_joint_state() try: for pts in way_points: candidate_sols = self.cartesian_to_ik(cartesian=pts) ik_point = self.kin.get_closest_joint_sol( current_joint=current_joint, candidate_sols=candidate_sols) current_joint = copy(ik_point) ik_point[ 0: 5] *= -1 # HACKS to convert values to Gazebo referene frame joint_way_points.append(ik_point) return joint_way_points except AssertionError: raise AssertionError( "No possible IK solutions found for coordinate: {0}". format(pts)) # Moves the robot by an axis by a delta distance current_cartesian = self.get_robot_cartesian_state() goal_cartesian = get_goal_cartesian_from_delta_dist( current_cartesian, axis, delta_dist) return plan_joint_way_points(current_cartesian, goal_cartesian) def get_robot_joint_state(self): # Current joint state of Mario return list(self.__robot_joint_state) def get_robot_cartesian_state(self): robot_joint_state = self.get_robot_joint_state() return self.cartesian_from_joint(robot_joint_state)
import rospy from std_msgs.msg import String from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory from trajectory_msgs.msg import JointTrajectoryPoint from geometry_msgs.msg import WrenchStamped import interpolate from scipy.interpolate import interp1d import matplotlib.pyplot as plt import numpy as np from ur_kin_py.kin import Kinematics kin = Kinematics('ur5') # or ur10 s = kin.forward([1.57, -0.15, -1.57, 0, 0, 0]) f = kin.forward([0, -0.15, -1.57, 0, 0, 0]) xs = s[0:3, 3] xf = f[0:3, 3] m = kin.forward([0.] * 6) def plan(xs, xf, v=[0, 0, 0], t=10): n = 100 global m global kin
def main(): global client, MODE_DEBUG, MODE_REDUCE_FULL try: if (len(sys.argv) > 1): if (sys.argv[1] == 'debug'): MODE_DEBUG = True print "MODE_DEBUG=" + str(MODE_DEBUG) if (len(sys.argv) > 2): if (sys.argv[2] == 'full'): MODE_REDUCE_FULL = False print "Mode Debug Full ? : " + str(MODE_REDUCE_FULL) c_print("Initialisation du Publisher /ur_driver/URScript", MODE_REDUCE_FULL) pub = rospy.Publisher('/ur_driver/URScript', String, queue_size=10) c_print("Initialisation du Publisher /wsg_50_driver/goal_position", MODE_REDUCE_FULL) pubGripper = rospy.Publisher('/wsg_50_driver/goal_position', Cmd, queue_size=10) c_print(str("Initialisation du noeud :" + NODE_NAME), MODE_REDUCE_FULL) rospy.init_node(NODE_NAME, anonymous=True, disable_signals=True) print "This program makes the robot move, choose a mode:" print " - Mode ROS (ROS FollowJointTrajectoryAction) : R" print " - Mode URSCRIPT (movej, servoj) : U" print " - Demo : D" print " - Kinematics : K" inp = raw_input("Mode? R/U/D/K: ")[0] if (inp == 'R'): client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction) c_print("Waiting for server...", MODE_REDUCE_FULL) client.wait_for_server() c_print("Connected to server") #move1() move_repeated() #move_disordered() #move_interrupt() elif (inp == 'U'): c_print(str("Instanciation Kinematic UR10"), MODE_REDUCE_FULL) kin = Kinematics('ur10') c_print(str("Attente d'un message de JointState"), MODE_REDUCE_FULL) joint_states = rospy.wait_for_message("joint_states", JointState) joints_pos = joint_states.position c_print("JointState.position: " + str(tuple(joints_pos)), MODE_REDUCE_FULL) x = kin.forward(np.array(joints_pos)) c_print(str("Declaration point de depart du bras"), True) c_print("First Pose " + str(x)) #Dessine un arc de cercle pour une translation de 10cm test_incr([0.00, 0.30, 0.00], x, kin, pub, pubGripper, 300) elif (inp == 'D'): pubGripper.publish("open", 70.0, 4.0) verif_move_gripper() c_print(str("Instanciation Kinematic UR10"), MODE_REDUCE_FULL) kin = Kinematics('ur10') c_print(str("Attente d'un message de JointState"), MODE_REDUCE_FULL) joint_states = rospy.wait_for_message("joint_states", JointState) joints_pos = joint_states.position c_print("JointState.position: " + str(tuple(joints_pos)), MODE_REDUCE_FULL) x = kin.forward(np.array(joints_pos)) c_print(str("Placement pour Pion 1"), True) x = [[0.00, -1.00, 0.00, 0.00], [0.00, 0.00, 1.00, 0.38], [-1.00, 0.00, 0.00, 0.19], [0., 0., 0., 1.]] c_print("First Pose " + str(x)) #print "Modif : ",x sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("close", 34.8, 4.0) verif_move_gripper() c_print(str("Deplacement pour Pion 1"), True) move_diag_right_top(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("open", 50.0, 4.0) verif_move_gripper() c_print(str("Position temporaire"), True) move_leave(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("open", 60.0, 4.0) verif_move_gripper() c_print(str("Placement pour Pion 2"), True) move_left(x) move_left(x) move_top(x) move_top(x) move_take(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("close", 34.8, 4.0) verif_move_gripper() c_print(str("Deplacement pour Pion 2"), True) move_diag_right_down(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("open", 50.0, 4.0) verif_move_gripper() c_print(str("Position temporaire"), True) move_leave(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" print send pub.publish(send) verif_move(sol) c_print(str("Placement Pion 1 mange Pion2"), True) move_diag_right_down(x) move_take(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("close", 34.8, 4.0) verif_move_gripper() c_print(str("Position temporaire"), True) move_leave(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) c_print(str("Placement pour manger Pion 2"), True) move_left(x) move_left(x) move_top(x) move_top(x) move_take(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("open", 50.0, 4.0) verif_move_gripper() c_print(str("Position temporaire"), True) move_leave(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) c_print(str("Pion qui meurt"), True) move_diag_right_down(x) move_take(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("close", 34.8, 4.0) verif_move_gripper() c_print(str("Position temporaire"), True) move_leave(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) c_print(str("Pion 2 mort"), True) move_kill(x) move_take(x) sol = kin.inverse(x, joints_pos) tmp = np.array2string(sol, precision=8, separator=',', suppress_small=True) send = "movej(" + tmp + ",t=5.0)" c_print(send, MODE_REDUCE_FULL) pub.publish(send) verif_move(sol) pubGripper.publish("open", 50.0, 4.0) verif_move_gripper() elif (inp == 'K'): TestKin.main() else: print "Halting program" except KeyboardInterrupt: rospy.signal_shutdown("KeyboardInterrupt") raise