def joint_position_callback(joints):
global plan_only
fixed_frame = rospy.get_param("/fixed_frame")
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = fixed_frame
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = fixed_frame
start_state.joint_state.name = []
start_state.joint_state.position = []
cons = Constraints()
cons.name = ""
i = 0
for dim in joints.start_joint.layout.dim:
start_state.joint_state.name.append(dim.label)
start_state.joint_state.position.append(joints.start_joint.data[i])
jc = JointConstraint()
jc.joint_name = dim.label
jc.position = joints.goal_joint.data[i]
jc.tolerance_above = 0.0001
jc.tolerance_below = 0.0001
jc.weight = 1.0
i = i + 1
cons.joint_constraints.append(jc)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = joints.group_name
move_group_goal.request = msg
if joints.plan_only:
plan_only = True
move_group_goal.planning_options.plan_only = True
else:
plan_only = False
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s" % e
def __init__(self):
self.bridge = CvBridge()
joint_state_topic = ['joint_states:=/iiwa/joint_states']
moveit_commander.roscpp_initialize(joint_state_topic)
rospy.Subscriber("/iiwa/joint_states", JointState, self.State_callback)
# Instantiate a RobotCommander object. This object is
# an interface to the robot as a whole.
self.robot = moveit_commander.RobotCommander()
self.group = moveit_commander.MoveGroupCommander("manipulator")
# rospy.sleep(2)
# self.scene = moveit_commander.PlanningSceneInterface('/iiwa/move_group/monitored_planning_scene')
# box_pose = PoseStamped()
# box_pose.header.frame_id = "world"
# box_pose.pose.position.x = 1.0
# box_pose.pose.orientation.w = 1.0
# self.scene.add_box("test", box_pose, size=(0.1, 0.2, 0.3))
# while not rospy.is_shutdown():
# rospy.sleep(1.0)
# for k in self.scene.__dict__.keys():
# print(k, self.scene.__dict__[k])
# # print(self.scene)
# print(self.scene.get_known_object_names())
# print(self.scene.get_attached_objects())
# exit()
self.group.set_max_velocity_scaling_factor(0.05)
self.group.set_max_acceleration_scaling_factor(0.05)
current_pose = self.group.get_current_pose(end_effector_link='iiwa_link_ee').pose
self._joint_efforts = 0
self._joint_vel = 0
self._joint_name = 0
self._header = None
pose = PoseStamped()
self.upright_constraints = Constraints()
self.upright_constraints.name = "upright"
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = self.group.get_planning_frame()
orientation_constraint.link_name = self.group.get_end_effector_link()
pose.pose.orientation.x = 1.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 0.0
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = .7#3.0
orientation_constraint.absolute_y_axis_tolerance = .7#3.0
orientation_constraint.absolute_z_axis_tolerance = 3.1415
#orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis
orientation_constraint.weight = 1
self.upright_constraints.orientation_constraints.append(orientation_constraint)
self.group.allow_replanning(True)
self.group.allow_looking(True)
workspace = [0.5,-0.3,0.15,0.7,0.2,0.25]
# self.group.set_workspace(workspace)
# self.group.set_path_constraints(self.upright_constraints)
self.traj_num = -1
self.im_num = 0
self.MAX_PATH_LENGTH = 15
def plan_to_goal(self,
x,
y,
z,
or_x=0.0,
or_y=-1.0,
or_z=0.0,
or_w=0.0,
start=None,
time=None):
try:
goal = PoseStamped()
goal.header.frame_id = "world"
if time is not None:
goal.header.stamp = time
#x, y, and z position
goal.pose.position.x = x
goal.pose.position.y = y
goal.pose.position.z = z
#Orientation as a quaternion
goal.pose.orientation.x = or_x
goal.pose.orientation.y = or_y
goal.pose.orientation.z = or_z
goal.pose.orientation.w = or_w
self.group.set_pose_target(goal)
if start is None:
self.group.set_start_state_to_current_state()
else:
self.group.set_start_state(start)
constraints = Constraints()
# constraints.orientation_constraints = orien_const
self.group.set_path_constraints(constraints)
traj = self.group.plan()
new_traj = RobotTrajectory()
new_traj.joint_trajectory.header = traj.joint_trajectory.header
new_traj.joint_trajectory.joint_names = traj.joint_trajectory.joint_names
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
print("Executing %d point trajectory" % n_points)
for i in range(n_points):
new_traj.joint_trajectory.points.append(JointTrajectoryPoint())
time_step = traj.joint_trajectory.points[
i].time_from_start / self.speed
new_traj.joint_trajectory.points[i].time_from_start = time_step
if len(traj.joint_trajectory.points[i].velocities) != n_joints:
print(traj.joint_trajectory.points[i].velocities)
for j in range(len(
traj.joint_trajectory.points[i].velocities)):
new_traj.joint_trajectory.points[i].velocities.append(
traj.joint_trajectory.points[i].velocities[j] *
self.speed)
# new_traj.joint_trajectory.points[i].accelerations.append(traj.joint_trajectory.points[i].accelerations[j] * self.speed * self.speed)
new_traj.joint_trajectory.points[i].positions.append(
traj.joint_trajectory.points[i].positions[j])
if time is not None:
new_traj.joint_trajectory.header.stamp = time - new_traj.joint_trajectory.points[
-1].time_from_start - rospy.Duration(0.0)
return new_traj
except Exception as e:
traceback.print_exc()
return None
def moveToPose(self,
pose_stamped,
gripper_frame,
tolerance=0.01,
wait=True,
**kwargs):
'''
Move the arm, based on a goal pose_stamped for the end effector.
:param pose_stamped: target pose to which we want to move
specified link to
:param gripper_frame: frame/link which we want to move
to the specified target.
:param tolerance: allowed tolerance in the final joint positions.
:param wait: if enabled, makes the fuctions wait until the
target joint position is reached
:type pose_stamped: ros message object of type PoseStamped
:type gripper_frame: string
:type tolerance: float
:type wait: bool
'''
# Check arguments
supported_args = ("max_velocity_scaling_factor", "planner_id",
"planning_time", "plan_only", "start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToPose: unsupported argument: %s", arg)
# Create goal
g = MoveGroupGoal()
pose_transformed = self._listener.transformPose(
self._fixed_frame, pose_stamped)
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance * tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose_transformed.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[
0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs[
"max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
result = self._action.get_result()
return processResult(result)
else:
rospy.loginfo('Failed while waiting for action result.')
return False
def both_arms_go_to_pose_goal(self):
# 设置动作对象变量,此处为both_arms
both_arms = self.both_arms
# 获取当前各轴转角
axis_angle = both_arms.get_current_joint_values()
# print axis_angle
# 获取当前末端执行器位置姿态
right_pose_goal = both_arms.get_current_pose(
end_effector_link="gripper_r_finger_r")
left_pose_goal = both_arms.get_current_pose(
end_effector_link="gripper_l_finger_r")
print right_pose_goal
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
if Rightfinger > -55:
right_joint_const.position = 0.024
else:
right_joint_const.position = 0
left_joint_const = JointConstraint()
left_joint_const.joint_name = "gripper_l_joint_r"
if Leftfinger > -32:
left_joint_const.position = 0.024
else:
left_joint_const.position = 0
# 添加末端姿态约束:
right_orientation_const = OrientationConstraint()
right_orientation_const.header = Header()
right_orientation_const.orientation = right_pose_goal.pose.orientation
right_orientation_const.link_name = "gripper_r_joint_r"
right_orientation_const.absolute_x_axis_tolerance = 0.6
right_orientation_const.absolute_y_axis_tolerance = 0.6
right_orientation_const.absolute_z_axis_tolerance = 0.6
right_orientation_const.weight = 1
left_orientation_const = OrientationConstraint()
left_orientation_const.header = Header()
left_orientation_const.orientation = left_pose_goal.pose.orientation
left_orientation_const.link_name = "gripper_l_joint_r"
left_orientation_const.absolute_x_axis_tolerance = 0.6
left_orientation_const.absolute_y_axis_tolerance = 0.6
left_orientation_const.absolute_z_axis_tolerance = 0.6
left_orientation_const.weight = 1
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const, left_joint_const]
# consts.orientation_constraints = [right_orientation_const, left_orientation_const]
both_arms.set_path_constraints(consts)
# # 设置动作对象目标位置姿态
# # 右臂
# right_pose_goal.pose.orientation.x = Right_Qux
# right_pose_goal.pose.orientation.y = Right_Quy
# right_pose_goal.pose.orientation.z = Right_Quz
# right_pose_goal.pose.orientation.w = Right_Quw
# right_pose_goal.pose.position.x = Neurondata[5]
# right_pose_goal.pose.position.y = Neurondata[3]
# right_pose_goal.pose.position.z = Neurondata[4]
# # 左臂
# left_pose_goal.pose.orientation.x = Left_Qux
# left_pose_goal.pose.orientation.y = Left_Quy
# left_pose_goal.pose.orientation.z = Left_Quz
# left_pose_goal.pose.orientation.w = Left_Quw
# left_pose_goal.pose.position.x = Neurondata[11]
# left_pose_goal.pose.position.y = Neurondata[9]
# left_pose_goal.pose.position.z = Neurondata[10]
# # 右臂
# right_pose_goal.pose.orientation.x = Right_Qux
# right_pose_goal.pose.orientation.y = Right_Quy
# right_pose_goal.pose.orientation.z = Right_Quz
# right_pose_goal.pose.orientation.w = Right_Quw
# right_pose_goal.pose.position.x = (1266/740)*(Neurondata[5]+0.28)-0.495
# right_pose_goal.pose.position.y = (1295/780)*(Neurondata[3]+0.56)-0.754
# right_pose_goal.pose.position.z = (1355/776)*(Neurondata[4]-0.054)-0.184
# # 左臂
# left_pose_goal.pose.orientation.x = Left_Qux
# left_pose_goal.pose.orientation.y = Left_Quy
# left_pose_goal.pose.orientation.z = Left_Quz
# left_pose_goal.pose.orientation.w = Left_Quw
# left_pose_goal.pose.position.x = (1266/850)*(Neurondata[11]+0.33)-0.495
# left_pose_goal.pose.position.y = (1295/720)*(Neurondata[9]+0.19)-0.541
# left_pose_goal.pose.position.z = (1355/745)*(Neurondata[10]-0.055)-0.184
# 右臂
right_pose_goal.pose.orientation.x = Right_Qux
right_pose_goal.pose.orientation.y = Right_Quy
right_pose_goal.pose.orientation.z = Right_Quz
right_pose_goal.pose.orientation.w = Right_Quw
right_pose_goal.pose.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053
right_pose_goal.pose.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12
right_pose_goal.pose.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47
# 左臂
left_pose_goal.pose.orientation.x = Left_Qux
left_pose_goal.pose.orientation.y = Left_Quy
left_pose_goal.pose.orientation.z = Left_Quz
left_pose_goal.pose.orientation.w = Left_Quw
left_pose_goal.pose.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053
left_pose_goal.pose.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12
left_pose_goal.pose.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47
# # 右臂
# right_pose_goal.pose.orientation.x = right_pose_goal.pose.orientation.x
# right_pose_goal.pose.orientation.y = right_pose_goal.pose.orientation.y
# right_pose_goal.pose.orientation.z = right_pose_goal.pose.orientation.z
# right_pose_goal.pose.orientation.w = right_pose_goal.pose.orientation.w
# right_pose_goal.pose.position.x = right_pose_goal.pose.position.x
# right_pose_goal.pose.position.y = right_pose_goal.pose.position.y
# right_pose_goal.pose.position.z = right_pose_goal.pose.position.z
# # 左臂
# left_pose_goal.pose.orientation.x = left_pose_goal.pose.orientation.x
# left_pose_goal.pose.orientation.y = left_pose_goal.pose.orientation.y
# left_pose_goal.pose.orientation.z = left_pose_goal.pose.orientation.z
# left_pose_goal.pose.orientation.w = left_pose_goal.pose.orientation.w
# left_pose_goal.pose.position.x = left_pose_goal.pose.position.x
# left_pose_goal.pose.position.y = left_pose_goal.pose.position.y
# left_pose_goal.pose.position.z = left_pose_goal.pose.position.z
# 设置动作组的两个目标点
both_arms.set_pose_target(right_pose_goal,
end_effector_link="gripper_r_finger_r")
both_arms.set_pose_target(left_pose_goal,
end_effector_link="gripper_l_finger_r")
# 规划和输出动作
traj = both_arms.plan()
both_arms.execute(traj, wait=False)
# # 清除路径约束
both_arms.clear_path_constraints()
# 确保输出停止
both_arms.stop()
def probe(self, nx, ny, dx, dy):
#Initialize arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
right_arm = moveit_commander.MoveGroupCommander('right_arm')
right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planning_time(15)
# Set constraints
rospy.sleep(2)
# Assuming you're facing the wall, looking at the robot
# And the robot's computer is to your left
# Then Wall 1 is the wall on "your" right
# Wall 2 is the wall to "your" left
# Wall 3 is the wall in the back
#raw_input("press any key to add wall 1")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = 0.
p.pose.position.z = 0.
scene.add_box("wall1", p, (0.1, 5, 5))
#raw_input("press any key to add wall 2")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = -1
p.pose.position.y = 0.
p.pose.position.z = 0.
scene.add_box("wall2", p, (0.1, 5, 5))
#raw_input("press any key to add wall 3")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0.7
p.pose.position.z = 0.
scene.add_box("wall3", p, (5, 0.1, 5))
#raw_input("press any key to add patient")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = self.table_x
p.pose.position.y = self.table_y
p.pose.position.z = self.table_z / 2.0
scene.add_box("patient", p, (0.3, 0.3, self.table_z / 2.0))
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 50
orien_const.weight = .5
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
ans = {} # start with empty dictionary
for i in range(-nx, nx + 1):
for j in range(-nx, nx + 1):
ans[(i, j)] = self.poke_at(right_arm, self.table_x + i * dx,
self.table_y + j * dy)
return ans
class PlannerInterface(object):
def __init__(self):
super(PlannerInterface, self).__init__()
# First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('PlannerInterface')
# Instantiate a `MoveGroupCommander`_ object. This object is an interface to
# a planning group (group of joints).
group_name = "arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
move_group.allow_replanning(True)# Set planning parameters
print("Arm Moveit Commander has been started")
self.move_group = move_group
def create_path(self): #This function is used to build a list of waypoints
move_group = self.move_group
# --- Getting pose message
wpose = Pose()
waypoints = []
# --- Adding points to follow in path
print(cblue + "\n\t === Acquire a trajectory === \n" + cend)
print "Current directory: ", dir
print "Files found: \n"
print '\n'.join(files)
readline.set_completer(completer)#active autocompletion on filenames
readline.parse_and_bind("tab: complete")
filename = raw_input(cyellow + "\nGetting path from file: " + cend)
filepath = "Trajectories/" + filename
#-- Test to check file availability
try:
way = open(filepath,'r')
except IOError:# in case of mispelling filename
print(cred + "Looks like the file doesnt exist" + cend)
return None,None
os.system('clear')
print(cgreen + "\n\t > - - Niryo One - Surmoul 3D - - < \n" + cend)
print("\n-> file : " + '\033[4m' + filename + cend + " - Modified : " + time.ctime(os.path.getmtime(filepath)))
line = way.readline()
nbr = 1
#-- Counting lines and looking for errors
while line:
nbr += 1
line = way.readline()
data = len(line.split(' '))
if(( data != 7 and data != 8) and line != ""):
print(cred + "Error line " + str(nbr) + " does not contain 3 positions + 4 quaternions values :" + cend)
print "contains %d values"%(data)
print line
return None,None
way.seek(0)
debug = False
if(filename == 'interactive.txt'):
debug = True
nbr = int(raw_input('Nbr of lines to read : '))
traveling_distance = 0 #to calculate travelling distance
bar = Bar('Processing waypoints', max=nbr - 1,width=10)
i = 0
distance_btwn_points = []
prev_x = 0
prev_y = 0
prev_z = 0
while(i <= nbr - 2):
i+=1
bar.next()
tab = way.readline().split(' ')
q1 = [float(tab[3]),float(tab[4]),float(tab[5]),float(tab[6])]
wpose.orientation.w = q1[0] #- Quaternion
wpose.orientation.x = q1[1]
wpose.orientation.y = q1[2]
wpose.orientation.z = q1[3]
wpose.position.x = float(tab[0]) + offsets_niryo[0] #- Position #
wpose.position.y = float(tab[1]) + offsets_niryo[1]
wpose.position.z = float(tab[2]) + offsets_niryo[2]
distance_btwn_points.append(sqrt(pow(wpose.position.x - prev_x,2) + pow(wpose.position.y - prev_y,2) + pow(wpose.position.z - prev_z,2)))
if(len(tab) == 8 ): # if gcode
if(not(int(tab[7])) and (distance_btwn_points[-1] != -2) ): # if extrusion 0 then retractation
distance_btwn_points[-1] = -2 #overwrite last point with retractation in mm
else:
traveling_distance += distance_btwn_points[-1]
prev_x = wpose.position.x
prev_y = wpose.position.y
prev_z = wpose.position.z
waypoints.append(copy.deepcopy(wpose))
#-- Outing trajectory---------------
if(divmod(i,5990)[1] == 0 or i == nbr - 1): #the trajectory is splitted in 5990 points, robot goes to a specific pose
#where he can wait then will execute next part of trajectory
wpose.position.x += -0.07
wpose.position.z+=0.05
wpose.orientation.x = 0 #- Quaternion
wpose.orientation.y = 0.259
wpose.orientation.z = 0
wpose.orientation.w = 0.966
waypoints.append(copy.deepcopy(wpose))
distance_btwn_points.append(0)
way.close()
bar.finish()
for i in range(0,len(distance_btwn_points)):
if(divmod(i,5990)[1] == 0):
distance_btwn_points[i] = 0.005 #5mm to empty and fill nozzle
error_way = 0
is_error = True
while(is_error):
temp_way = copy.deepcopy(waypoints)
is_error=False
for i in range(0,len(waypoints) - 1): #- delete duplicated points in the path
if(waypoints[i] == waypoints[i + 1]):
del temp_way[i - error_way]
error_way +=1
is_error=True
#print "line %d"%(i)#-debug
waypoints = copy.deepcopy(temp_way)
print "-Duplicated points on path- errors fixed : %d" % (error_way)
traveling_distance = round(traveling_distance * 0.209 , 2) # filament diameter 1.75mm / nozzle 0.8mm
a,b = divmod(traveling_distance,1000)
print "Trajectory points found: %d" % (len(waypoints) - 1)
print "Approx. Necessary Filament: %2dm %3dmm" % (a,b * 100)
return waypoints, distance_btwn_points
def plan_cartesian_path(self, max_tries, waypoints, start_joints): # Call the planner
move_group = self.move_group
#------------------------------------------------------------------------------------------------------------
#- inforamtions on move_group http://docs.ros.org/jade/api/moveit_commander/html/move__group_8py_source.html
#- http://docs.ros.org/melodic/api/moveit_msgs/html/index-msg.html
#------------------------------------------------------------------------------------------------------------
#-- Check for robot stability
if(n.get_learning_mode()):
print "Make sure that Niryo is not in learning mode and in a safe position close to where you want to print"
raw_input("Then press enter..")
#-- Saving start state
tab_joints = [start_joints[0], start_joints[1],start_joints[2],start_joints[3],start_joints[4],start_joints[5]]
#-- Sending start state
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = ['joint_1', 'joint_2','joint_3','joint_4','joint_5','joint_6']
joint_state.position = tab_joints
initial_state = RbState()
initial_state.joint_state = joint_state
move_group.set_start_state(initial_state)
#-- Parameters
fraction = 0.0
tries = 0
max_tries = max_tries #maximum tries allowed
eef_step = 1.0 #eef_step at 1.0 considering gcode is already an interpolation
velocity = 0.06 #velocity scaling factor applied to max velocity
#print "\n --- Computing parameters ---"
#print "| Max tries authorized : %2d \n| Eef step : %.4f \n| Velocity :
#%3d %%" %(max_tries,eef_step,velocity*100)
#print " ------------------------------\n"
#-- Call cartesian service
try:
moveit_cartesian_path = rospy.ServiceProxy('compute_cartesian_path', GetCartesianPath)
except rospy.ServiceException,e:
print("Service call failed:",e)
return(None)
#-- Computation
best_frac = 0.0
t_in = time.time()
while(fraction < 1.0 and tries < max_tries):
rospy.wait_for_service('compute_cartesian_path',2) #wait for service to be ready
response = moveit_cartesian_path(Header(), initial_state, 'arm', 'tool_link',waypoints, eef_step, 0.0, True, Constraints())#send request
#------------------------------------------------------------------------------------------------------------
#- see http://docs.ros.org/melodic/api/moveit_msgs/html/srv/GetCartesianPath.html for more info
#------------------------------------------------------------------------------------------------------------
tries+=1
fraction = round(response.fraction,5)
if(fraction < 1.0): #in case solution is not complete we print iteration info in red (missing points)
print(cred + "---try:" + str(tries) + "\t---completed:" + str(fraction * 100) + "% error code: " + str(response.error_code) + cend)
if(fraction > best_frac): #saving best plan
best_plan = response.solution
best_frac = response.fraction
else:
print("---try:" + str(tries) + "\t---completed:" + str(fraction * 100) + "%")#printing iterations
best_plan = response.solution
best_frac = response.fraction
time.sleep(0.5) #time sleep to cut CPU usage and let some cooling time
t_out = time.time()
c_time = round(t_out - t_in,2)
print "\n==> tries: %d complete: %d %% in: %.2f sec" % (tries, best_frac * 100,c_time)#print process results
if(best_frac < 1.0):
print "In most cases if the service doesnt compute 100% of the trajectory it is due to unreachable points or orientation"
print "The problem is occuring at line (approx) : %d" % (round(fraction * len(waypoints)))
#-- Scaling speeds for Niryo One
if(velocity < 1.0 and best_frac == 1.0):
print"==> Retiming trajectory at %3d%% speed.." % (velocity * 100)
best_plan = move_group.retime_trajectory(initial_state, best_plan, velocity) #ref_state_in, plan, velocity sc
#-- Case where absolutely no points can be computed
if(best_frac == 0):
return 0, tab_joints, best_frac # this returns a 0 for empty trajectory, the original start joints and best frac which value is 0
else:
expect_m, expect_s = divmod(best_plan.joint_trajectory.points[-1].time_from_start.secs , 60)
expect_h, expect_m = divmod(expect_m , 60)
print "\nExpected printing time : %dh%02dm%02ds" % (expect_h,expect_m,expect_s)
end_joints = list(best_plan.joint_trajectory.points[-1].positions)# returns last joint position in case of using multiple trajectories that are
# following each other
return best_plan , end_joints , best_frac
def right_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
if Rightfinger > -55:
right_joint_const.position = 0.024
else:
right_joint_const.position = 0
right_joint_const.weight = 1.0
# 限制1轴转动
right_joint1_const = JointConstraint()
right_joint1_const.joint_name = "yumi_joint_1_r"
right_joint1_const.position = 0
right_joint1_const.tolerance_above = 120
right_joint1_const.tolerance_below = 0
right_joint1_const.weight = 1.0
# 限制2轴转动
right_joint2_const = JointConstraint()
right_joint2_const.joint_name = "yumi_joint_2_r"
right_joint2_const.position = 0
right_joint2_const.tolerance_above = 0
right_joint2_const.tolerance_below = 150
right_joint2_const.weight = 1.0
# 限制3轴转动
right_joint3_const = JointConstraint()
right_joint3_const.joint_name = "yumi_joint_3_r"
right_joint3_const.position = 0
right_joint3_const.tolerance_above = 35
right_joint3_const.tolerance_below = 55
right_joint3_const.weight = 1.0
# 限制4轴转动
right_joint4_const = JointConstraint()
right_joint4_const.joint_name = "yumi_joint_4_r"
right_joint4_const.position = 0
right_joint4_const.tolerance_above = 60
right_joint4_const.tolerance_below = 75
right_joint4_const.weight = 1.0
# 限制5轴转动
right_joint5_const = JointConstraint()
right_joint5_const.joint_name = "yumi_joint_5_r"
right_joint5_const.position = 40
right_joint5_const.tolerance_above = 50
right_joint5_const.tolerance_below = 20
right_joint5_const.weight = 1.0
# 限制6轴转动
right_joint6_const = JointConstraint()
right_joint6_const.joint_name = "yumi_joint_6_r"
right_joint6_const.position = 0
right_joint6_const.tolerance_above = 10
right_joint6_const.tolerance_below = 35
right_joint6_const.weight = 1.0
# 限制7轴转动
right_joint7_const = JointConstraint()
right_joint7_const.joint_name = "yumi_joint_7_r"
right_joint7_const.position = -10
right_joint7_const.tolerance_above = 0
right_joint7_const.tolerance_below = 160
right_joint7_const.weight = 1.0
# 限制末端位移
right_position_const = PositionConstraint()
right_position_const.header = Header()
right_position_const.link_name = "gripper_r_joint_r"
right_position_const.target_point_offset.x = 0.5
right_position_const.target_point_offset.y = -0.5
right_position_const.target_point_offset.z = 1.0
right_position_const.weight = 1.0
# 添加末端姿态约束:
right_orientation_const = OrientationConstraint()
right_orientation_const.header = Header()
right_orientation_const.orientation = pose_goal.orientation
right_orientation_const.link_name = "gripper_r_finger_r"
right_orientation_const.absolute_x_axis_tolerance = 0.50
right_orientation_const.absolute_y_axis_tolerance = 0.25
right_orientation_const.absolute_z_axis_tolerance = 0.50
right_orientation_const.weight = 100
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
# consts.orientation_constraints = [right_orientation_const]
# consts.position_constraints = [right_position_const]
right_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Right_Qux
pose_goal.orientation.y = Right_Quy
pose_goal.orientation.z = Right_Quz
pose_goal.orientation.w = Right_Quw
pose_goal.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053
pose_goal.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12
pose_goal.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47
right_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = 0.1644
# pose_goal.orientation.y = 0.3111
# pose_goal.orientation.z = 0.9086
# pose_goal.orientation.w = 0.2247
# pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053
# pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12
# pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47
# right_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = pose_goal.orientation.x
# pose_goal.orientation.y = pose_goal.orientation.y
# pose_goal.orientation.z = pose_goal.orientation.z
# pose_goal.orientation.w = pose_goal.orientation.w
# pose_goal.position.x = pose_goal.position.x
# pose_goal.position.y = pose_goal.position.y - 0.01
# pose_goal.position.z = pose_goal.position.z
# right_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = right_arm.plan()
right_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
right_arm.clear_pose_targets()
# 清除路径约束
right_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
right_arm.stop()
def pick(self, target_name, grasp_position, pre_grasp_distance,
post_grasp_retreat):
pre_grasp_posture = JointTrajectory()
grasp_posture = JointTrajectory()
pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
limit = {'dist': 0.01, 'r': 0.15, 'p': 0.15, 'y': 0.15}
constraints = Constraints()
oc = OrientationConstraint()
oc.header.frame_id = REFERENCE_FRAME
oc.link_name = 'j2s7s300_end_effector'
oc.absolute_x_axis_tolerance = limit['r'] # radian
oc.absolute_y_axis_tolerance = limit['p']
oc.absolute_z_axis_tolerance = limit['y']
oc.weight = 1.0
oc.orientation = grasp_position.pose.orientation
constraints.orientation_constraints.append(deepcopy(oc))
result = False
replan_times = 1
replan_state = True
while replan_state and replan_times <= 5:
(pre_grasp_path, fraction) = self.get_path(grasp_position.pose,
0.01,
constraints=constraints)
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
self.state = STATE.STOP
return
if fraction >= 0.92:
print "Pre_grasp_approach..."
self.arm.execute(pre_grasp_path)
result = self.check(grasp_position.pose, limit)
replan_state = False
replan_times += 1
if result:
result = False
if self.arm.attach_object(
target_name, self.arm.get_end_effector_link(),
[
"j2s7s300_end_effector", "j2s7s300_link_finger_1",
"j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_2",
"j2s7s300_link_finger_tip_2", "j2s7s300_link_finger_3",
"j2s7s300_link_finger_tip_3", "j2s7s300_joint_finger_1",
"j2s7s300_joint_finger_2", "j2s7s300_joint_finger_3"
]):
rospy.logwarn("Attach request sent successfully!")
else:
raise Exception("Can't send attach request!")
print "Grasping..."
self.gripper.set_named_target("Close")
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
self.arm.detach_object(target_name)
self.back_to_init_pose()
self.state = STATE.STOP
return
self.gripper.go()
rospy.sleep(1)
post_grasp_position = self.get_retreat_point(
grasp_position.pose, post_grasp_retreat)
print "post_grasp_position: %s\n" % post_grasp_position
replan_times = 1
replan_state = True
while replan_state and replan_times <= 5:
(retreat_path,
fraction) = self.get_path(post_grasp_position,
0.005,
constraints=constraints)
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
self.arm.detach_object(target_name)
self.back_to_init_pose(state=1)
self.state = STATE.STOP
return
if fraction > 0.8:
print "Retreating..."
self.arm.execute(retreat_path)
result = self.check(post_grasp_position, limit)
replan_state = False
replan_times += 1
if not result:
self.state = STATE.GRASP_RETREAT_ERR
else:
self.state = STATE.PRE_GRASP_ERR # pre_grasp planning failed
if result:
self.state = STATE.PICK_FINISH
self.arm.clear_path_constraints()
def place(self, target_name, place_position, pre_place_distance,
post_place_retreat):
limit = {'dist': 0.01, 'r': 0.10, 'p': 0.10, 'y': 0.10}
constraints = Constraints()
oc = OrientationConstraint()
oc.header.frame_id = REFERENCE_FRAME
oc.link_name = 'j2s7s300_end_effector'
oc.absolute_x_axis_tolerance = limit['r'] # radian
oc.absolute_y_axis_tolerance = limit['p']
oc.absolute_z_axis_tolerance = limit['y']
oc.weight = 0.85
oc.orientation = place_position.pose.orientation
constraints.orientation_constraints.append(deepcopy(oc))
result = False
replan_times = 1
replan_state = True
while replan_state and replan_times <= 5:
(place_path, fraction) = self.get_path(place_position.pose,
0.01,
constraints=constraints)
if fraction >= 0.95:
print "Pre_place_approach..."
self.arm.execute(place_path)
result = self.check(place_position.pose, limit)
replan_state = False
replan_times += 1
if result:
result = False
print "Placing..."
self.gripper.set_named_target("Open")
self.gripper.go()
self.arm.detach_object(target_name)
rospy.sleep(1)
post_place_position = self.get_retreat_point(
place_position.pose, post_place_retreat)
replan_state = True
replan_times = 1
while replan_state and replan_times <= 5:
(retreat_path,
fraction) = self.get_path(post_place_position,
0.01,
constraints=constraints)
if fraction > 0.8:
self.arm.execute(retreat_path)
result = self.check(post_place_position, limit)
replan_state = False
replan_times += 1
if not result:
self.state = STATE.PLACE_RETREAT_ERR
else:
self.state = STATE.PRE_PLACE_ERR
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
rospy.logwarn("Can't cancel task after place action start!")
if result:
self.state = STATE.PLACE_FINISH
self.arm.clear_path_constraints()
def cw3_example_script():
"""
This script will go through the main aspects of moveit and the components you will need to complete the coursework.
You can find more information on
"""
# Initialize moveit_commander and rospy node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface')
# Initialize moveit scene interface (woeld surrounding the robot)
scene = moveit_commander.PlanningSceneInterface()
# Robot contains the entire state of the robot (iiw a and shadow hand)
global group, base_group, arm_group
# request_prin = rospy.ServiceProxy('set_extruder_printing', SetBool)
msg_prit = SetBoolRequest()
request_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander('robot')
arm_group = moveit_commander.MoveGroupCommander('arm')
# gripeer_group = moveit_commander.MoveGroupCommander('gripper')
base_group = moveit_commander.MoveGroupCommander('base')
# Create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
extruder_publisher = rospy.Publisher('set_extruder_rate',
Float32,
queue_size=20)
msg_extrude = Float32()
msg_extrude = 5.0
extruder_publisher.publish(msg_extrude)
group.allow_replanning(True)
group.allow_looking(True)
# Set the number of planning attempts to find better solution
group.set_num_planning_attempts(10)
################################################
###Add obstacle
rospy.sleep(0.5)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = 0
box_pose.pose.position.y = 3
box_pose.pose.position.z = 0.01
box_name = "wall"
scene.add_box(box_name, box_pose, size=(2, 0.01, 0.01))
rospy.sleep(0.5)
rospy.sleep(0.5)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = -0.75
box_pose.pose.position.y = 3.5
box_pose.pose.position.z = 0.2
box_one = "box_1"
scene.add_box(box_one, box_pose, size=(0.5, 0.5, 0.5))
rospy.sleep(0.5)
rospy.sleep(0.5)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = -0
box_pose.pose.position.y = 2.7
box_pose.pose.position.z = 0.2
box_two = "box_2"
scene.add_box(box_two, box_pose, size=(0.5, 0.5, 0.5))
rospy.sleep(0.5)
rospy.sleep(0.5)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = 0.75
box_pose.pose.position.y = 3.5
box_pose.pose.position.z = 0.2
box_three = "box_3"
scene.add_box(box_three, box_pose, size=(0.5, 0.5, 0.5))
rospy.sleep(0.5)
#############################################################
point_list = []
for i in range(11):
point_list.append((i * 0.2 - 1, 3, 0.1))
print(point_list)
#############################################################
########################################################################
##Add constrain
pose = group.get_current_pose(group.get_end_effector_link())
constraints = Constraints()
# orientation_constraint = OrientationConstraint()
# orientation_constraint.header = pose.header
# orientation_constraint.link_name = group.get_end_effector_link()
# orientation_constraint.orientation = pose.pose.orientation
# orientation_constraint.absolute_x_axis_tolerance = 0.1
# orientation_constraint.absolute_y_axis_tolerance = 0.1
# orientation_constraint.absolute_z_axis_tolerance = 2*pi
# current_orientation_list = [pose.pose.orientation.x,
# pose.pose.orientation.y,
# pose.pose.orientation.z,
# pose.pose.orientation.w]
# # get euler angle from quaternion
# (roll, pitch, yaw) = euler_from_quaternion(current_orientation_list)
# pitch = pi
# roll = 0
# orientation_constraint.weight = 1
# [orientation_constraint.orientation.x, orientation_constraint.orientation.y, orientation_constraint.orientation.z, orientation_constraint.orientation.w] = \
# quaternion_from_euler(roll, pitch, yaw)
# constraints.orientation_constraints.append(orientation_constraint)
# group.set_path_constraints(constraints)
#### joint constraints
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_1'
joint_constraint.position = 169 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1
constraints.joint_constraints.append(joint_constraint)
group.set_path_constraints(constraints)
#######################################################################
while len(point_list) > 1:
# Move the robot point to first point and find the height
initial_plan = move_to_initial(point_list[1])
joint_goal = group.get_current_joint_values()
head = initial_plan.joint_trajectory.header
robot_state = robot.get_current_state()
# print(robot.get_current_state().joint_state.position)
robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \
tuple(robot_state.joint_state.position[7:])
resp = request_fk(head, [group.get_end_effector_link()], robot_state)
current_pose = group.get_current_pose().pose
current_pose.orientation = resp.pose_stamped[0].pose.orientation
(current_pose.position.x, current_pose.position.y,
current_pose.position.z) = point_list[0]
group.set_pose_target(current_pose)
group.go()
# Move the robot to the center of the striaght line to make sure Way point method can be executed
# Way points
waypoints = []
wpose = group.get_current_pose().pose
# Add the current pose to make sure the path is smooth
waypoints.append(copy.deepcopy(wpose))
success_num = 0
for point_num in range(len(point_list)):
(wpose.position.x, wpose.position.y,
wpose.position.z) = point_list[point_num]
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list): group.execute(execute_plan)
elif success_num == 0:
break
else:
# execute success plan
msg_prit.data = True
group.execute(execute_plan)
break
msg_prit.data = False
# Delete the points what already execute
if success_num > 0:
del (point_list[0:success_num - 1])
# Add obstacle after printing (need to revise)
#############################################################
print 'all finish'
# When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def main():
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_node')
#Set up the right gripper
right_gripper = robot_gripper.Gripper('right')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
right_gripper.calibrate()
rospy.sleep(2.0)
#Initialize both arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
# left_arm = moveit_commander.MoveGroupCommander('left_arm')
right_arm = moveit_commander.MoveGroupCommander('right_arm')
# left_arm.set_planner_id('RRTConnectkConfigDefault')
# left_arm.set_planning_time(10)
right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planning_time(10)
#First goal pose ------------------------------------------------------
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.2
goal_1.pose.position.y = -0.5
goal_1.pose.position.z = 0.2
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_1)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input(
'Press <Enter> to move the right arm to goal pose 1 (path constraints are never enforced during this motion): '
)
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = -0.3
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_2)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the right arm to goal pose 2: ')
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
#Third goal pose -----------------------------------------------------
rospy.sleep(2.0)
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.6
goal_3.pose.position.y = -0.1
goal_3.pose.position.z = 0.1
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_3)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the right arm to goal pose 3: ')
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
def build(self, tf_timeout=rospy.Duration(5.0)):
"""Builds the goal message.
To set a pose or joint goal, call set_pose_goal or set_joint_goal
before calling build. To add a path orientation constraint, call
add_path_orientation_constraint first.
Args:
tf_timeout: rospy.Duration. How long to wait for a TF message. Only
used with pose goals.
Returns: moveit_msgs/MoveGroupGoal
"""
goal = MoveGroupGoal()
# Set start state
goal.request.start_state = copy.deepcopy(self.start_state)
# Set goal constraint
if self._pose_goal is not None:
self._tf_listener.waitForTransform(self._pose_goal.header.frame_id,
self.fixed_frame,
rospy.Time.now(), tf_timeout)
try:
pose_transformed = self._tf_listener.transformPose(
self.fixed_frame, self._pose_goal)
except (tf.LookupException, tf.ConnectivityException):
return None
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self.fixed_frame
c1.position_constraints[0].link_name = self.gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [self.tolerance * self.tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(self._pose_goal.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self.fixed_frame
c1.orientation_constraints[
0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = self.gripper_frame
c1.orientation_constraints[
0].absolute_x_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_y_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_z_axis_tolerance = self.tolerance
c1.orientation_constraints[0].weight = 1.0
goal.request.goal_constraints.append(c1)
elif self._joint_names is not None:
c1 = Constraints()
for i in range(len(self._joint_names)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = self._joint_names[i]
c1.joint_constraints[i].position = self._joint_positions[i]
c1.joint_constraints[i].tolerance_above = self.tolerance
c1.joint_constraints[i].tolerance_below = self.tolerance
c1.joint_constraints[i].weight = 1.0
goal.request.goal_constraints.append(c1)
# Set path constraints
goal.request.path_constraints.orientation_constraints = self._orientation_constraints
# Set trajectory constraints
# Set planner ID (name of motion planner to use)
goal.request.planner_id = self.planner_id
# Set group name
goal.request.group_name = self.group_name
# Set planning attempts
goal.request.num_planning_attempts = self.num_planning_attempts
# Set planning time
goal.request.allowed_planning_time = self.allowed_planning_time
# Set scaling factors
goal.request.max_acceleration_scaling_factor = self.max_acceleration_scaling_factor
goal.request.max_velocity_scaling_factor = self.max_velocity_scaling_factor
# Set planning scene diff
goal.planning_options.planning_scene_diff = copy.deepcopy(
self.planning_scene_diff)
# Set is plan only
goal.planning_options.plan_only = self.plan_only
# Set look around
goal.planning_options.look_around = self.look_around
# Set replanning options
goal.planning_options.replan = self.replan
goal.planning_options.replan_attempts = self.replan_attempts
goal.planning_options.replan_delay = self.replan_delay
return goal
def get_trajectory_constraints(self):
""" Get the actual trajectory constraints in form of a moveit_msgs.msgs.Constraints """
c = Constraints()
c_str = self._g.get_trajectory_constraints()
conversions.msg_from_string(c, c_str)
return c
correct_js = copy.deepcopy(current_joint_states)
result = fk.getFK('arm_right_7_link', current_joint_states.name,
correct_js.position, 'base_link')
rospy.loginfo("Result of current robot pose FK is: " + str(result))
rs = RobotState()
rs.joint_state = correct_js
drs = DisplayRobotState()
drs.state = rs
rs_pub.publish(drs)
rospy.loginfo("Published current robot state")
rospy.sleep(2.0)
c = Constraints()
jc = JointConstraint()
jc.joint_name = 'arm_right_1_link'
jc.position = 0.0
jc.tolerance_above = 0.00001
jc.tolerance_below = 0.00001
jc.weight = 1.0
c.joint_constraints.append(jc)
rospy.loginfo("Result without constraints:")
resultik = ik.getIK("right_arm", "right_arm_7_link",
result.pose_stamped[0], False, 0, rs)
rospy.loginfo(str(resultik))
rs = RobotState()
rs.joint_state = resultik.solution.joint_state
def print_pointlist(self, point_list, future_print_status=False):
startime = datetime.datetime.now()
# Save original points list
full_point_list = copy.deepcopy(point_list)
full_point_array = np.delete(np.array(full_point_list), 2, axis=1)
self.target_list = full_point_list
if future_print_status: self.future_printing_status = True
# Constraints
pose = self.group.get_current_pose(self.group.get_end_effector_link())
constraints = Constraints()
# joint constraints
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_1'
joint_constraint.position = 169 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_4'
joint_constraint.position = 150 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
self.group.set_path_constraints(constraints)
# Orientation constrains
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = self.group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 2 * pi
orientation_constraint.absolute_y_axis_tolerance = 2 * pi
orientation_constraint.absolute_z_axis_tolerance = 2 * pi
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
# Record how many points has already finished
finsih_num = 0
print_num = 0
index_check = 0
while len(point_list) > 0:
print('New Plan, points left:', len(point_list), point_list)
# Move the robot point to first point and find the height
if len(point_list) > 1:
initial_plan = self.move_to_initial(point_list[1])
else:
initial_plan = self.move_to_initial(point_list[0])
# joint_goal = self.group.get_current_joint_values()
head = initial_plan.joint_trajectory.header
robot_state = self.robot.get_current_state()
# print(robot_state.joint_state)
robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \
tuple(robot_state.joint_state.position[7:]) # the joints for the wheel
resp = self.request_fk(head, [self.group.get_end_effector_link()],
robot_state)
current_pose = self.group.get_current_pose().pose
current_pose.orientation = resp.pose_stamped[0].pose.orientation
(current_pose.position.x, current_pose.position.y,
current_pose.position.z) = point_list[0]
self.group.set_pose_target(current_pose)
self.group.go()
# Move the robot to the center of the striaght line to make sure Way point method can be executed
# Way points
waypoints = []
wpose = self.group.get_current_pose().pose
# Add the current pose to make sure the path is smooth
waypoints.append(copy.deepcopy(wpose))
success_num = 0
for point_num in range(len(point_list)):
(wpose.position.x, wpose.position.y,
wpose.position.z) = point_list[point_num]
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
print 'Adding the first planing point, and fraction is', fraction
executing_state = 0
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list):
self.group.execute(execute_plan, wait=False)
self.msg_print.data = True
executing_state = 1
success_num += 1
elif success_num == 0:
break
else:
# execute success plan
self.msg_print.data = True
self.group.execute(execute_plan, wait=False)
executing_state = 1
break
## 2nd loop
## always re-plan and check for obstacle while executing waypoints
executed_waypoint_index = 0 # initial value of nothing
success_point_list = point_list[:success_num]
print('when plan success, move_group_status:',
self.move_group_execution_status, 'success_plan_number:',
success_num)
if executing_state == 1:
success_planned_waypoint_array = np.delete(np.array(
point_list[:success_num]),
2,
axis=1)
# print 'success planned waypoint\n', success_planned_waypoint_array
print 'status', self.waypoint_execution_status
while self.waypoint_execution_status != 3:
if point_list == []: break
if self.waypoint_execution_status == 4:
# aborted state
print 'stop and abort waypoint execution'
self.msg_print.data = False
self.group.stop()
executing_state = 0
break
current_ee_pose = self.group.get_current_pose().pose
current_ee_position_array = np.array([
current_ee_pose.position.x, current_ee_pose.position.y
])
executed_waypoint_index = self.check_executed_waypoint_index(
success_planned_waypoint_array,
current_ee_position_array)
# print 'executed latest index', executed_waypoint_index
index_check = self.check_executed_waypoint_index(
full_point_array, current_ee_position_array)
self.further_printing_number = index_check
print 'index:', index_check, 'way_point index', executed_waypoint_index
if future_print_status == True:
# Check for enclosure
self.base_group.set_position_target(
[0, 0, 0.05],
self.base_group.get_end_effector_link())
result = self.base_group.plan()
self.base_group.clear_pose_targets()
if len(result.joint_trajectory.points) == 0:
print('Check enclosure failed')
else:
print('Check enclosure successful')
## Replan to check for dynamic obstacle
waypoints = []
# Add the current pose to make sure the path is smooth, get latest pose
current_ee_pose = self.group.get_current_pose().pose
waypoints.append(copy.deepcopy(current_ee_pose))
# discard the executed waypoints
new_point_list = point_list[
executed_waypoint_index:success_num]
for k in new_point_list:
(current_ee_pose.position.x,
current_ee_pose.position.y,
current_ee_pose.position.z) = k
waypoints.append(copy.deepcopy(current_ee_pose))
(plan2, fraction2) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
print 'Dynamic check fraction:', fraction2
if fraction2 < 1:
## new obstacle appear
# print 'executed latest index', executed_waypoint_index
# print 'fraction value', fraction,'\n'
print 'new obstacle appeared to be in the path'
self.group.stop()
self.msg_print.data = False
executing_state = 0
break
rospy.sleep(2)
print 'status:', self.waypoint_execution_status, 'executed_index:', executed_waypoint_index, 'success_num:', success_num
if self.waypoint_execution_status == 3:
# waypoint successfully printed
# self.print_list_visualize(point_list[:success_num])
self.rviz_visualise_marker(point_list[:success_num])
del (point_list[:success_num - 1])
elif self.waypoint_execution_status == 2 or 4:
# state 2 = preempted, state 4 = aborted.
# only printed partial waypoint
# self.print_list_visualize(point_list[:executed_waypoint_index+1])
if executed_waypoint_index > 0: # at index 0, it might have not print the point 0-1 edge successfully.
self.rviz_visualise_marker(
point_list[:executed_waypoint_index + 1])
del (point_list[:executed_waypoint_index]
) # delete up till whatever is executed
self.msg_print.data = False
if point_list == []: rospy.sleep(2)
self.group.stop()
# Delete the points what already execute
# if success_num > 0:
# Add obstacle after printing (need to revise)
self.group.set_path_constraints(None)
finshtime = datetime.datetime.now()
print(finshtime - startime).seconds
full_point_list_x = [base[0] for base in full_point_list]
full_point_list_y = [base[1] for base in full_point_list]
plt3 = plt.figure("Printing result", figsize=(6, 6))
plt.plot(self.re_position_x, self.re_position_y, 'b')
plt.plot(full_point_list_x, full_point_list_y, 'ro')
plt.xlim(min(full_point_list_x) - 0.1, max(full_point_list_x) + 0.1)
plt.ylim(min(full_point_list_y) - 0.1, max(full_point_list_y) + 0.1)
plt.legend(['Printing result', 'ground truth'],
fontsize=8,
bbox_to_anchor=(1.0, 1))
plt.xlabel("X axis (m)")
plt.ylabel("y axis (m)")
plt.title('Printing result')
plt.show()
plt3.savefig('result.png', dpi=plt3.dpi)
print('All finish, time:', (finshtime - startime).seconds)
def __init__(self):
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.contraints = Constraints()
is_initialized = False
while (not is_initialized):
try:
# self.group = moveit_commander.MoveGroupCommander("arm", wait_for_servers=1.0)
self.group = moveit_commander.MoveGroupCommander("arm")
is_initialized = True
except RuntimeError:
is_initialized = False
rospy.sleep(0.5)
self.group.set_planning_time(
1) # Limit the planning time to a second. (Default : 5 seconds)
rospy.loginfo("Initialized...")
self.traj_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=1)
rospy.loginfo("============ Reference planning frame: %s" %
self.group.get_planning_frame())
rospy.loginfo("============ Reference end_effector link: %s" %
self.group.get_end_effector_link())
# self.sub_callback_pointcloud = rospy.Subscriber('/move_group/filtered_cloud', PointCloud2, self.callback_pointcloud)
self.sub_add_obstacle = rospy.Subscriber('/add_obstacle', String,
self.add_obstacle)
self.sub_del_obstacle = rospy.Subscriber('/del_obstacle', String,
self.del_obstacle)
self.sub_del_all_obstacles = rospy.Subscriber('/del_all_obstacles',
String,
self.del_all_obstacles)
self.action_plan_execute_pose = actionlib.SimpleActionServer(
'/plan_and_execute_pose',
PlanExecutePoseAction,
execute_cb=self.plan_execute_pose_cb,
auto_start=False)
self.action_plan_execute_pose.start()
self.action_plan_execute_named_pose = actionlib.SimpleActionServer(
'/plan_and_execute_named_pose',
PlanExecuteNamedPoseAction,
execute_cb=self.plan_execute_named_pose_cb,
auto_start=False)
self.action_plan_execute_named_pose.start()
self.action_plan_execute_pose_w_constraints = actionlib.SimpleActionServer(
'/plan_and_execute_pose_w_joint_constraints',
PlanExecutePoseConstraintsAction,
execute_cb=self.plan_execute_pose_constraints_cb,
auto_start=False)
self.action_plan_execute_pose_w_constraints.start()
rospy.loginfo('%s ready...' % rospy.get_name())
self.box_names_arr = []
self.box_pose = geometry_msgs.msg.PoseStamped()
self.collision_objects = []
def left_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
left_arm = self.left_arm
# 获取当前末端执行器位置姿态
pose_goal = left_arm.get_current_pose().pose
# 限制末端夹具运动
left_joint_const = JointConstraint()
left_joint_const.joint_name = "gripper_l_joint_r"
if Leftfinger > -32:
left_joint_const.position = 0.024
else:
left_joint_const.position = 0
left_joint_const.weight = 1.0
# 限制末端位移
left_position_const = PositionConstraint()
left_position_const.header = Header()
left_position_const.link_name = "gripper_l_joint_r"
left_position_const.target_point_offset.x = 0.5
left_position_const.target_point_offset.y = -0.5
left_position_const.target_point_offset.z = 1.0
left_position_const.weight = 1.0
# # 限制1轴转动
left_joint1_const = JointConstraint()
left_joint1_const.joint_name = "yumi_joint_1_l"
left_joint1_const.position = 0
left_joint1_const.tolerance_above = 1.76
left_joint1_const.tolerance_below = 0
left_position_const.weight = 1.0
# 限制2轴转动
left_joint2_const = JointConstraint()
left_joint2_const.joint_name = "yumi_joint_2_l"
left_joint2_const.position = 0
left_joint2_const.tolerance_above = 0
left_joint2_const.tolerance_below = 150
left_joint2_const.weight = 1.0
# 限制3轴转动
left_joint3_const = JointConstraint()
left_joint3_const.joint_name = "yumi_joint_3_l"
left_joint3_const.position = 0
left_joint3_const.tolerance_above = 35
left_joint3_const.tolerance_below = 55
left_joint3_const.weight = 1.0
# 限制4轴转动
left_joint4_const = JointConstraint()
left_joint4_const.joint_name = "yumi_joint_4_l"
left_joint4_const.position = 0
left_joint4_const.tolerance_above = 60
left_joint4_const.tolerance_below = 75
left_joint4_const.weight = 1.0
# 限制5轴转动
right_joint5_const = JointConstraint()
right_joint5_const.joint_name = "yumi_joint_5_l"
right_joint5_const.position = 40
right_joint5_const.tolerance_above = 50
right_joint5_const.tolerance_below = 20
right_joint5_const.weight = 1.0
# 限制6轴转动
left_joint6_const = JointConstraint()
left_joint6_const.joint_name = "yumi_joint_6_l"
left_joint6_const.position = 0
left_joint6_const.tolerance_above = 10
left_joint6_const.tolerance_below = 35
left_joint6_const.weight = 1.0
# 限制7轴转动
left_joint7_const = JointConstraint()
left_joint7_const.joint_name = "yumi_joint_7_l"
left_joint7_const.position = -10
left_joint7_const.tolerance_above = 0
left_joint7_const.tolerance_below = 160
left_joint7_const.weight = 1.0
# 限制末端位移
left_position_const = PositionConstraint()
left_position_const.header = Header()
left_position_const.link_name = "gripper_l_joint_r"
left_position_const.target_point_offset.x = 0.5
left_position_const.target_point_offset.y = 0.25
left_position_const.target_point_offset.z = 0.5
left_position_const.weight = 1.0
# 添加末端姿态约束:
left_orientation_const = OrientationConstraint()
left_orientation_const.header = Header()
left_orientation_const.orientation = pose_goal.orientation
left_orientation_const.link_name = "gripper_l_finger_r"
left_orientation_const.absolute_x_axis_tolerance = 0.5
left_orientation_const.absolute_y_axis_tolerance = 0.25
left_orientation_const.absolute_z_axis_tolerance = 0.5
left_orientation_const.weight = 1
# 施加全约束
consts = Constraints()
consts.joint_constraints = [left_joint_const]
# consts.orientation_constraints = [left_orientation_const]
# consts.position_constraints = [left_position_const]
left_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Left_Qux
pose_goal.orientation.y = Left_Quy
pose_goal.orientation.z = Left_Quz
pose_goal.orientation.w = Left_Quw
pose_goal.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053
pose_goal.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12
pose_goal.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47
left_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = pose_goal.orientation.x
# pose_goal.orientation.y = pose_goal.orientation.y
# pose_goal.orientation.z = pose_goal.orientation.z
# pose_goal.orientation.w = pose_goal.orientation.w
# pose_goal.position.x = pose_goal.position.x
# pose_goal.position.y = pose_goal.position.y - 0.01
# pose_goal.position.z = pose_goal.position.z
# left_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = left_arm.plan()
left_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
left_arm.clear_pose_targets()
# 清除路径约束
left_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
left_arm.stop()
def moveToPose(self,
pose_stamped,
gripper_frame,
tolerance=0.01,
wait=True,
**kwargs):
# Check arguments
supported_args = ("max_velocity_scaling_factor", "planner_id",
"planning_time", "plan_only", "start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToPose: unsupported argument: %s", arg)
# Create goal
g = MoveGroupGoal()
pose_transformed = self._listener.transformPose(
self._fixed_frame, pose_stamped)
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance * tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose_transformed.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[
0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs[
"max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
return self._action.get_result()
else:
return None
# Add Object to the Planning Scene
rospy.loginfo("Add Objects to the Planning Scene...")
box_pose = PoseStamped()
box_pose.header.frame_id = group.get_planning_frame()
box_pose.pose.position.x = 0.3
box_pose.pose.position.y = -0.3
box_pose.pose.position.z = -0.25
box_pose.pose.orientation.w = 1.0
scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5))
rospy.sleep(2)
rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names()))
# Set Path Constraint
constraints = Constraints()
constraints.name = "down"
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = group.get_planning_frame()
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.orientation = pose_target_1.orientation
orientation_constraint.absolute_x_axis_tolerance = 3.1415
orientation_constraint.absolute_y_axis_tolerance = 0.05
orientation_constraint.absolute_z_axis_tolerance = 0.05
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
group.set_path_constraints(constraints)
rospy.loginfo("Get Path Constraints:\n{}".format(
def moveToJointPosition(self,
joints,
positions,
tolerance=0.01,
wait=True,
**kwargs):
# Check arguments
supported_args = ("max_velocity_scaling_factor", "planner_id",
"planning_scene_diff", "planning_time", "plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
# 1. fill in workspace_parameters
# 2. fill in start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in goal_constraints
c1 = Constraints()
for i in range(len(joints)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = joints[i]
c1.joint_constraints[i].position = positions[i]
c1.joint_constraints[i].tolerance_above = tolerance
c1.joint_constraints[i].tolerance_below = tolerance
c1.joint_constraints[i].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in path constraints
# 5. fill in trajectory constraints
# 6. fill in planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in group name
g.request.group_name = self._group
# 8. fill in number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs[
"max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
try:
g.planning_options.planning_scene_diff = kwargs[
"planning_scene_diff"]
except KeyError:
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
return self._action.get_result()
else:
return None
def on_enter(self, userdata):
self._param_error = False
self._planning_failed = False
self._control_failed = False
self._success = False
#Parameter check
if self._srdf_param is None:
self._param_error = True
return
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn(
'Unable to parse given SRDF parameter: /robot_description_semantic'
)
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib[
'name']:
robot = r
userdata.robot_name = robot # Save robot name to output key
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' %
self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
self._move_group = c.attrib[
'group'] # Set move group name in case it was not defined
userdata.config_name = config # Save configuration name to output key
userdata.move_group = self._move_group # Save move_group to output key
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' %
self._config_name)
return 'param_error'
try:
self._joint_config = [
float(j.attrib['value']) for j in config.iter('joint')
]
self._joint_names = [
str(j.attrib['name']) for j in config.iter('joint')
]
userdata.joint_values = self._joint_config # Save joint configuration to output key
userdata.joint_names = self._joint_names # Save joint names to output key
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' %
str(e))
return 'param_error'
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
userdata.action_topic = self._action_topic # Save action topic to output key
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
def on_enter(self, userdata):
self.return_code = None
self.status_text = None
self.current_action_topic = self.given_action_topic
# Retrieve the relevant data
try:
if (self.given_action_topic is None):
self.current_action_topic = userdata.action_topic
self.move_group = userdata.move_group
self.joint_values = userdata.joint_values
self.joint_names = userdata.joint_names
if (len(self.joint_values) != len(self.joint_names)):
self.status_text = 'Mismatch in joint names and values (%d vs. %d) -' % (
len(self.joint_values), len(self.joint_names))
self.return_code = 'param_error'
Logger.logerr(self.status_text)
return
except Exception as e:
self.status_text = 'Failed to get relevant user data -\n%s' % (
str(e))
self.return_code = 'param_error'
Logger.logerr(self.status_text)
return
try:
if (self.client is None):
self.client = ProxyActionClient(
{self.current_action_topic: MoveGroupAction},
self.wait_duration)
if not self.client.is_available(self.current_action_topic):
self.client.setupClient(self.current_action_topic,
MoveGroupAction, self.wait_duration)
if not self.client.is_available(self.current_action_topic):
self.status_text = 'Action client is not available for %s' % (
self.current_action_topic)
self.return_code = 'param_error'
Logger.logerr(self.status_text)
return
except Exception as e:
self.status_text = 'Failed to set up the action client for %s\n%s' % (
self.current_action_topic, str(e))
self.return_code = 'param_error'
Logger.logerr(self.status_text)
return
try:
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self.move_group
action_goal.request.allowed_planning_time = self.allowed_planning_time
action_goal.request.start_state.is_diff = True # Flags start state as empty to use current state on server
action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True # Flags start state as empty to use current state on server
goal_constraints = Constraints()
for i in range(len(self.joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self.joint_names[i],
position=self.joint_values[i],
tolerance_above=self.joint_tolerance,
tolerance_below=self.joint_tolerance,
weight=self.constraint_weight))
action_goal.request.goal_constraints.append(goal_constraints)
if (self.timeout_duration > rospy.Duration(0.0)):
self.timeout_target = rospy.Time.now(
) + self.timeout_duration + rospy.Duration(
self.allowed_planning_time)
else:
self.timeout_target = None
self.client.send_goal(self.current_action_topic, action_goal)
except Exception as e:
self.status_text = 'Failed to send action goal for group - %s\n%s' % (
self.move_group, str(e))
self.return_code = 'planning_failed'
Logger.logerr(self.status_text)
return
def moveToJointPosition(self,
joints,
positions,
tolerance=0.01,
wait=True,
**kwargs):
'''
Move the arm to set of joint position goals
:param joints: joint names for which the target position
is specified.
:param positions: target joint positions
:param tolerance: allowed tolerance in the final joint positions.
:param wait: if enabled, makes the fuctions wait until the
target joint position is reached
:type joints: list of string element type
:type positions: list of float element type
:type tolerance: float
:type wait: bool
'''
# Check arguments
supported_args = ("max_velocity_scaling_factor", "planner_id",
"planning_scene_diff", "planning_time", "plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
# 1. fill in workspace_parameters
# 2. fill in start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in goal_constraints
c1 = Constraints()
for i in range(len(joints)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = joints[i]
c1.joint_constraints[i].position = positions[i]
c1.joint_constraints[i].tolerance_above = tolerance
c1.joint_constraints[i].tolerance_below = tolerance
c1.joint_constraints[i].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in path constraints
# 5. fill in trajectory constraints
# 6. fill in planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in group name
g.request.group_name = self._group
# 8. fill in number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs[
"max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
try:
g.planning_options.planning_scene_diff = kwargs[
"planning_scene_diff"]
except KeyError:
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
result = self._action.get_result()
return processResult(result)
else:
rospy.loginfo('Failed while waiting for action result.')
return False
def main():
#Initialize the node
rospy.init_node('moveit_client')
# Create the SimpleActionClient, passing the type of the action
# (MoveGroupAction) to the constructor.
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
# Wait until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
goal = MoveGroupGoal()
#----------------Construct the goal message (start)----------------
joint_names = ['head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
#Set parameters for the planner
goal.request.group_name = 'both_arms'
goal.request.num_planning_attempts = 1
goal.request.allowed_planning_time = 5.0
#Define the workspace in which the planner will search for solutions
goal.request.workspace_parameters.min_corner.x = -1
goal.request.workspace_parameters.min_corner.y = -1
goal.request.workspace_parameters.min_corner.z = -1
goal.request.workspace_parameters.max_corner.x = 1
goal.request.workspace_parameters.max_corner.y = 1
goal.request.workspace_parameters.max_corner.z = 1
goal.request.start_state.joint_state.header.frame_id = "base"
#Set the start state for the trajectory
goal.request.start_state.joint_state.name = joint_names
goal.request.start_state.joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
goal.request.start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#Tell MoveIt whether to execute the trajectory after planning it
goal.planning_options.plan_only = True
#Set the goal position of the robot
#Note that the goal is specified with a collection of individual
#joint constraints, rather than a vector of joint angles
arm_joint_names = joint_names[1:]
target_joint_angles = [0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
tolerance = 0.0001
consts = []
for i in range(len(arm_joint_names)):
const = JointConstraint()
const.joint_name = arm_joint_names[i]
const.position = target_joint_angles[i]
const.tolerance_above = tolerance
const.tolerance_below = tolerance
const.weight = 1.0
consts.append(const)
goal.request.goal_constraints.append(Constraints(name='', joint_constraints=consts))
#---------------Construct the goal message (end)-----------------
# Send the goal to the action server.
client.send_goal(goal)
# Wait for the server to finish performing the action.
client.wait_for_result()
# Print out the result of executing the action
print(client.get_result())
def get_path_constraints(self):
""" Get the acutal path constraints in form of a moveit_msgs.msgs.Constraints """
c = Constraints()
c_str = self._g.get_path_constraints()
conversions.msg_from_string(c, c_str)
return c
def print_pointlist(self, point_list):
# Get differential of way point list
differential = self.get_way_point_differential(point_list)
differential_num = [0]
differential_num = differential_num + differential
eef_rotation_change = [0, 0]
for i in range(2, len(differential_num)):
eef_rotation_change.append(differential_num[i] -
differential_num[i - 1])
pose = self.group.get_current_pose(self.group.get_end_effector_link())
constraints = Constraints()
#### joint constraints
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_1'
joint_constraint.position = 169 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1
constraints.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_4'
joint_constraint.position = 150 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1
constraints.joint_constraints.append(joint_constraint)
self.group.set_path_constraints(constraints)
# Orientation constrains
# constraints = Constraints()
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = self.group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 2 * pi
orientation_constraint.absolute_y_axis_tolerance = 2 * pi
orientation_constraint.absolute_z_axis_tolerance = 2 * pi
constraints.orientation_constraints.append(orientation_constraint)
while len(point_list) > 1:
# Move the robot point to first point and find the height
initial_plan = self.move_to_initial(point_list[1])
joint_goal = self.group.get_current_joint_values()
head = initial_plan.joint_trajectory.header
robot_state = self.robot.get_current_state()
# print(robot.get_current_state().joint_state.position)
robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \
tuple(robot_state.joint_state.position[7:])
resp = self.request_fk(head, [self.group.get_end_effector_link()],
robot_state)
current_pose = self.group.get_current_pose().pose
current_pose.orientation = resp.pose_stamped[0].pose.orientation
(current_pose.position.x, current_pose.position.y,
current_pose.position.z) = point_list[0]
self.group.set_pose_target(current_pose)
self.group.go()
# Move the robot to the center of the striaght line to make sure Way point method can be executed
# Way points
waypoints = []
wpose = self.group.get_current_pose().pose
# Add the current pose to make sure the path is smooth
waypoints.append(copy.deepcopy(wpose))
success_num = 0
for point_num in range(len(point_list)):
(wpose.position.x, wpose.position.y,
wpose.position.z) = point_list[point_num]
(roll, pitch, yaw) = euler_from_quaternion([
wpose.orientation.x, wpose.orientation.y,
wpose.orientation.z, wpose.orientation.w
])
# [wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \
# quaternion_from_euler(roll, pitch, yaw + eef_rotation_change[point_num])
# print(yaw + eef_rotation_change[point_num])
[wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \
quaternion_from_euler(roll, pitch, yaw)
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list):
self.group.execute(execute_plan)
self.print_list_visualize(point_list)
elif success_num == 0:
break
else:
# execute success plan
self.msg_prit.data = True
self.group.execute(execute_plan)
self.print_list_visualize(point_list[:success_num])
break
self.msg_prit.data = False
# Delete the points what already execute
if success_num > 0:
del (point_list[0:success_num - 1])
del (eef_rotation_change[0:success_num - 1])
# Add obstacle after printing (need to revise)
self.group.set_path_constraints(None)
print 'all finish'
