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