def set_pose_constraints(self, tol_joint1, tol_joint2, tol_joint3):
angle_constraints = Constraints()
joint_constraint = JointConstraint()
angle_constraints.name = "angle"
joint_constraint.position = self.joint1_con
joint_constraint.tolerance_above = tol_joint1
joint_constraint.tolerance_below = tol_joint1
joint_constraint.weight = 0.1
joint_constraint.joint_name = "arm_shoulder_pan_joint"
angle_constraints.joint_constraints.append(joint_constraint)
joint_constraint2 = JointConstraint()
joint_constraint2.position = self.joint2_con
joint_constraint2.tolerance_above = tol_joint2
joint_constraint2.tolerance_below = tol_joint2
joint_constraint2.weight = 0.2
joint_constraint2.joint_name = "arm_wrist_2_joint"
angle_constraints.joint_constraints.append(joint_constraint2)
joint_constraint3 = JointConstraint()
joint_constraint3.position = self.joint3_con
joint_constraint3.tolerance_above = tol_joint3
joint_constraint3.tolerance_below = tol_joint3
joint_constraint3.weight = 0.3
joint_constraint3.joint_name = "arm_wrist_1_joint"
angle_constraints.joint_constraints.append(joint_constraint3)
group.set_path_constraints(angle_constraints)
def limit_base_joint_constraint(): ur5_constraints = Constraints() shoulder_pan_joint_constraint = JointConstraint() shoulder_lift_joint_constraint = JointConstraint() wrist_1_joint_constraint = JointConstraint() shoulder_pan_joint_constraint.joint_name = "shoulder_pan_joint" shoulder_pan_joint_constraint.position = 0 shoulder_pan_joint_constraint.tolerance_above = 3.14 shoulder_pan_joint_constraint.tolerance_below = 3.14 shoulder_pan_joint_constraint.weight = 1 shoulder_lift_joint_constraint.joint_name = "shoulder_lift_joint" shoulder_lift_joint_constraint.position = 0 shoulder_lift_joint_constraint.tolerance_above = 0 shoulder_lift_joint_constraint.tolerance_below = 3.14 shoulder_lift_joint_constraint.weight = 1 wrist_1_joint_constraint.joint_name = "wrist_1_joint" wrist_1_joint_constraint.position = 0 wrist_1_joint_constraint.tolerance_above = 3.14 wrist_1_joint_constraint.tolerance_below = 3.14 wrist_1_joint_constraint.weight = 1 ur5_constraints.joint_constraints.append(shoulder_pan_joint_constraint) ur5_constraints.joint_constraints.append(shoulder_lift_joint_constraint) ur5_constraints.joint_constraints.append(wrist_1_joint_constraint) return ur5_constraints
def main(argv):
client = actionlib.SimpleActionClient(
'/plan_and_execute_pose_w_joint_constraints',
PlanExecutePoseConstraintsAction)
client.wait_for_server()
goal = PlanExecutePoseConstraintsGoal()
try:
goal.target_pose.header.frame_id = argv[0]
goal.target_pose.pose.position.x = float(argv[1])
goal.target_pose.pose.position.y = float(argv[2])
goal.target_pose.pose.position.z = float(argv[3])
roll = float(argv[4]) * math.pi / 180.0
pitch = float(argv[5]) * math.pi / 180.0
yaw = float(argv[6]) * math.pi / 180.0
quat = quaternion_from_euler(roll, pitch, yaw)
print quat
goal.target_pose.pose.orientation.x = quat[0]
goal.target_pose.pose.orientation.y = quat[1]
goal.target_pose.pose.orientation.z = quat[2]
goal.target_pose.pose.orientation.w = quat[3]
joint_constraint_arm1_joint = JointConstraint()
joint_constraint_arm1_joint.joint_name = "arm1_joint"
joint_constraint_arm1_joint.position = float(0.0)
joint_constraint_arm1_joint.tolerance_above = float(
30) * math.pi / 180.0
joint_constraint_arm1_joint.tolerance_below = float(
30) * math.pi / 180.0
joint_constraint_arm1_joint.weight = 1.0
joint_constraint_arm6_joint = JointConstraint()
joint_constraint_arm6_joint.joint_name = "arm6_joint"
joint_constraint_arm6_joint.position = float(0.0)
joint_constraint_arm6_joint.tolerance_above = float(
10) * math.pi / 180.0
joint_constraint_arm6_joint.tolerance_below = float(
10) * math.pi / 180.0
joint_constraint_arm6_joint.weight = 1.0
goal.joint_constraints.append(joint_constraint_arm1_joint)
goal.joint_constraints.append(joint_constraint_arm6_joint)
except ValueError:
quit()
client.send_goal(goal)
client.wait_for_result()
print client.get_result()
def add_robot_constraints():
constraint = Constraints()
constraint.name = "camera constraint"
roll_constraint = OrientationConstraint()
# 'base_link' is equal to the world link
roll_constraint.header.frame_id = 'world'
# The link that must be oriented upwards
roll_constraint.link_name = "camera"
roll_constraint.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0,np.pi/3,0))
# Allow rotation of 45 degrees around the x and y axis
roll_constraint.absolute_x_axis_tolerance = np.pi #Allow max rotation of x degrees
roll_constraint.absolute_y_axis_tolerance = np.pi
roll_constraint.absolute_z_axis_tolerance = np.pi/2
# The roll constraint is the only constraint
roll_constraint.weight = 1
#constraint.orientation_constraints = [roll_constraint]
joint_1_constraint = JointConstraint()
joint_1_constraint.joint_name = "joint_1"
joint_1_constraint.position = 0
joint_1_constraint.tolerance_above = np.pi/2
joint_1_constraint.tolerance_below = np.pi/2
joint_1_constraint.weight = 1
joint_4_constraint = JointConstraint()
joint_4_constraint.joint_name = "joint_4"
joint_4_constraint.position = 0
joint_4_constraint.tolerance_above = np.pi/2
joint_4_constraint.tolerance_below = np.pi/2
joint_4_constraint.weight = 1
joint_5_constraint = JointConstraint()
joint_5_constraint.joint_name = "joint_5"
joint_5_constraint.position = np.pi/2
joint_5_constraint.tolerance_above = np.pi/2
joint_5_constraint.tolerance_below = np.pi/2
joint_5_constraint.weight = 1
joint_6_constraint = JointConstraint()
joint_6_constraint.joint_name = "joint_6"
joint_6_constraint.position = np.pi-0.79
joint_6_constraint.tolerance_above = np.pi
joint_6_constraint.tolerance_below = np.pi
joint_6_constraint.weight = 1
constraint.joint_constraints = [joint_1_constraint, joint_6_constraint]
return constraint
def create_move_group_joints_goal(self,
joint_names,
joint_values,
group="right_arm",
plan_only=False):
""" Creates a move_group goal based on pose.
@arg joint_names list of strings of the joint names
@arg joint_values list of digits with the joint values
@arg group string representing the move_group group to use
@arg plan_only bool to for only planning or planning and executing
@return MoveGroupGoal with the desired contents"""
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
for name, value in zip(joint_names, joint_values):
joint_c = JointConstraint()
joint_c.joint_name = name
joint_c.position = value
joint_c.tolerance_above = 0.01
joint_c.tolerance_below = 0.01
joint_c.weight = 1.0
goal_c.joint_constraints.append(joint_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 5
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True
moveit_goal.request.group_name = group
return moveit_goal
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._joint_config = userdata.joint_values
self._joint_names = userdata.joint_names
self._sub = ProxySubscriberCached({self._position_topic: JointState})
self._current_position = []
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
# 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)
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._planning_failed = False
self._control_failed = False
self._success = False
self._config_name = userdata.config_name # Currently not used
self._robot_name = userdata.robot_name # Currently not used
self._move_group = userdata.move_group
self._action_topic = userdata.action_topic
self._joint_config = userdata.joint_values
self._joint_names = userdata.joint_names
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
# 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 setUp(self):
# create a action client of move group
self._move_client = SimpleActionClient('move_group', MoveGroupAction)
self._move_client.wait_for_server()
moveit_commander.roscpp_initialize(sys.argv)
group_name = moveit_commander.RobotCommander().get_group_names()[0]
group = moveit_commander.MoveGroupCommander(group_name)
# prepare a joint goal
self._goal = MoveGroupGoal()
self._goal.request.group_name = group_name
self._goal.request.max_velocity_scaling_factor = 0.1
self._goal.request.max_acceleration_scaling_factor = 0.1
self._goal.request.start_state.is_diff = True
goal_constraint = Constraints()
joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
joint_names = group.get_active_joints()
for i in range(len(joint_names)):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[i]
joint_constraint.position = joint_values[i]
joint_constraint.weight = 1.0
goal_constraint.joint_constraints.append(joint_constraint)
self._goal.request.goal_constraints.append(goal_constraint)
self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
def send_move_group_joint_space_goal(self, move_group, joint_names, joint_values, plan_only=None):
'''
Retrieve the current move group action configuration and send the move request
@param move_group : string specifying a particular move group
@param joint_names : list of joint names for command
@param joint_values : list of joint values for target
'''
# We should have already set this pairing up during initialization
action_topic = ProxyMoveItClient._move_group_clients_[move_group]
# Get the latest setup of of the MoveGroup goal data
goal = MoveGroupGoal()
goal.request = copy.deepcopy(ProxyMoveItClient._motion_plan_requests[move_group])
goal.planning_options = copy.deepcopy(ProxyMoveItClient._planning_options[move_group])
if (plan_only is not None):
goal.planning_options.plan_only = plan_only
goal_constraints = Constraints()
for i in range(len(joint_names)):
jc = ProxyMoveItClient._joint_constraints[move_group][joint_names[i]]
goal_constraints.joint_constraints.append(JointConstraint(
joint_name=joint_names[i],
position=joint_values[i],
tolerance_above=jc.tolerance_above,
tolerance_below=jc.tolerance_above,
weight=jc.weight))
goal.request.goal_constraints.append(goal_constraints)
ProxyMoveItClient._action_clients[action_topic].send_goal(action_topic, goal)
def execute(self, userdata):
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
userdata.move_it_joint_goal = MoveGroupGoal()
goal_c = Constraints()
for name, value in zip(userdata.manip_joint_names, userdata.manip_goal_joint_pose):
joint_c = JointConstraint()
joint_c.joint_name = name
joint_c.position = value
joint_c.tolerance_above = 0.01
joint_c.tolerance_below = 0.01
joint_c.weight = 1.0
goal_c.joint_constraints.append(joint_c)
userdata.move_it_joint_goal.request.goal_constraints.append(goal_c)
userdata.move_it_joint_goal.request.num_planning_attempts = 5
userdata.move_it_joint_goal.request.allowed_planning_time = 5.0
userdata.move_it_joint_goal.planning_options.plan_only = False #False = Plan + Execute ; True = Plan only
userdata.move_it_joint_goal.planning_options.planning_scene_diff.is_diff = True
userdata.move_it_joint_goal.request.group_name = userdata.manip_joint_group
rospy.loginfo('Group Name: ' + userdata.manip_joint_group)
rospy.loginfo('Joints name: ' + str(userdata.manip_joint_names))
rospy.loginfo('Joints Values: ' + str(userdata.manip_goal_joint_pose))
rospy.loginfo('GOAL TO SEND IS:... ' + str(userdata.move_it_joint_goal))
return 'succeeded'
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._action_topic = userdata.move_group_prefix + userdata.action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._move_group = userdata.move_group
self._joint_names = None
self._joint_names = userdata.joint_names
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_values[i]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
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 __init__(self, frame, ns = ''):
# self.scene_pub = PlanningScenePublisher(ns)
self.move_group_client = SimpleActionClient("/{}/move_group".format(ns), MoveGroupAction)
self.move_group_client.wait_for_server()
self.move_group_msg = MoveGroupGoal()
#Create the message for the request
self.move_group_msg.request = MotionPlanRequest()
self.move_group_msg.request.workspace_parameters.header.frame_id = frame
self.move_group_msg.request.workspace_parameters.min_corner.x = -50
self.move_group_msg.request.workspace_parameters.min_corner.y = -50
self.move_group_msg.request.workspace_parameters.min_corner.z = 0
self.move_group_msg.request.workspace_parameters.max_corner.x = 50
self.move_group_msg.request.workspace_parameters.max_corner.y = 50
self.move_group_msg.request.workspace_parameters.max_corner.z = 50
# Target definition
self.move_group_msg.request.goal_constraints.append(Constraints())
self.constraints = JointConstraint()
self.constraints.tolerance_above = 0.001
self.constraints.tolerance_below = 0.001
self.constraints.weight = 1.0
for i in range(0,7):
self.move_group_msg.request.goal_constraints[0].joint_constraints.append(copy.deepcopy(self.constraints))
self.move_group_msg.planning_options.planning_scene_diff.is_diff = True
self.move_group_msg.planning_options.plan_only = True
def _cmd_to_request(self, robot):
"""Transforms the gripper command to a MotionPlanRequest."""
req = MotionPlanRequest()
# Set general info
req.planner_id = "PTP"
req.group_name = self._planning_group
req.max_velocity_scaling_factor = self._vel_scale * robot.global_motion_factor
req.max_acceleration_scaling_factor = self._acc_scale * robot.global_motion_factor
req.allowed_planning_time = 1.0
# Set an empty diff as start_state => the current state is used by the planner
req.start_state.is_diff = True
# create goal constraints
goal_constraints = Constraints()
if isinstance(self._goal, (float, int, long)):
joint_names = robot._robot_commander.get_group(self._planning_group).get_active_joints()
if len(joint_names) != 1:
raise IndexError("PG70 should have only one joint. But group " + req.group_name +
" contains " + str(len(joint_names)) + " joints.")
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[0]
joint_constraint.position = float(self._goal)
joint_constraint.weight = 1
goal_constraints.joint_constraints.append(joint_constraint)
else:
raise NotImplementedError("Unknown type of goal is given.")
req.goal_constraints.append(goal_constraints)
return req
def add_way_points(self, waypoints):
arm = self.arm
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
right_joint_const.position = 0
right_joint_const.weight = 1.0
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
arm.set_path_constraints(consts)
wpose = arm.get_current_pose().pose
wpose.orientation.x = Qux
wpose.orientation.y = Quy
wpose.orientation.z = Quz
wpose.orientation.w = Quw
wpose.position.x = a[5]
wpose.position.y = a[3]
wpose.position.z = a[4]
waypoints.append(copy.deepcopy(wpose))
# print waypoints
# print "----------------\n"
return waypoints
def right_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
# Create a path constraint for the arm
# UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
joint_const = JointConstraint()
joint_const.joint_name = "gripper_r_joint_r"
joint_const.position = 0
consts = Constraints()
consts.joint_constraints = [joint_const]
right_arm.set_path_constraints(consts)
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# print (a)
# print (Qux, Quy, Quz, Quw)
# 设置动作对象目标位置姿态
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]
pose_goal.position.y = Neurondata[3]
pose_goal.position.z = Neurondata[4]
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.stop()
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
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
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')]
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
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]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
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 init_constraint():
joint_constraint = JointConstraint()
joint_constraint.joint_name = "shoulder_pan_joint"
joint_constraint.position = 0
joint_constraint.tolerance_above = 0.7854
joint_constraint.tolerance_below = 0.7854
joint_constraint.weight = 1
base_constraint.joint_constraints.append(joint_constraint)
def _do_ik(self, px, py, pz, arm):
"""
Does inverse kinematics for a given arm and given position.
This solver is collision aware.
Args:
px, py, pz (float): Cartesian coordinates for position of arm end
effector.
arm (str): Which arm ("right_arm" or "left_arm") to do ik for.
Returns:
list: List of joint configuration for specific arm. Returns an
empty list if unable to do IK.
"""
IK_INFO_NAME = "/pr2_%s_kinematics/get_ik_solver_info" % (arm)
IK_NAME = "/compute_ik"
ik_solver_info_service_proxy = rospy.ServiceProxy(IK_INFO_NAME, GetKinematicSolverInfo)
ik_info_req = GetKinematicSolverInfoRequest()
rospy.loginfo("LightningTest: do_ik: Waiting for %s" % (IK_INFO_NAME))
rospy.wait_for_service(IK_INFO_NAME)
rospy.loginfo("About to get IK info.")
ik_info_res = ik_solver_info_service_proxy(ik_info_req)
rospy.loginfo("Got IK Solver info.")
ik_solver_service_proxy = rospy.ServiceProxy(IK_NAME, GetPositionIK)
ik_solve_req = GetPositionIKRequest()
ik_solve_req.ik_request.group_name = arm
ik_solve_req.ik_request.timeout = rospy.Duration(5.0)
ik_solve_req.ik_request.ik_link_name = "%s_wrist_roll_link" % (arm[0])
ik_solve_req.ik_request.pose_stamped.header.frame_id = "odom_combined"
ik_solve_req.ik_request.pose_stamped.pose.position.x = px
ik_solve_req.ik_request.pose_stamped.pose.position.y = py
ik_solve_req.ik_request.pose_stamped.pose.position.z = pz
ik_solve_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
ik_solve_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
ik_solve_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
ik_solve_req.ik_request.pose_stamped.pose.orientation.w = 1.0;
# TODO: Consider retrieving current robto state.
ik_solve_req.ik_request.robot_state.joint_state.name = ik_info_res.kinematic_solver_info.joint_names;
for i in xrange(len(ik_info_res.kinematic_solver_info.joint_names)):
joint_limits = ik_info_res.kinematic_solver_info.limits[i]
ik_solve_req.ik_request.robot_state.joint_state.position.append((joint_limits.min_position + joint_limits.max_position)/2.0)
joint_constraint = JointConstraint()
joint_constraint.joint_name = ik_info_res.kinematic_solver_info.joint_names[i]
joint_constraint.position = (joint_limits.max_position + joint_limits.min_position) / 2.0
joint_constraint.tolerance_above = (joint_limits.max_position - joint_limits.min_position) / 2.0
joint_constraint.tolerance_below = joint_constraint.tolerance_above
ik_solve_req.ik_request.constraints.joint_constraints.append(joint_constraint)
rospy.loginfo("LightningTest: do_ik: Waiting for IK service.")
rospy.wait_for_service(IK_NAME)
ik_solve_res = ik_solver_service_proxy(ik_solve_req)
if ik_solve_res.error_code.val == ik_solve_res.error_code.SUCCESS:
return ik_solve_res.solution.joint_state.position[30:37]
else:
rospy.loginfo("Lightning tester: cannot move to sampled position...trying another position")
return []
def plan_trajectory(self, start_point, goal_point, planner_number, joint_names, group_name, planning_time, planner_config_name, plan_type='pfs'):
"""
Given a start and goal point, plan by classical planner.
Args:
start_point (list of float): A starting joint configuration.
goal_point (list of float): A goal joint configuration.
planner_number (int): The index of the planner to be used as
returned by acquire_planner().
joint_names (list of str): The name of the joints corresponding to
start_point and goal_point.
group_name (str): The name of the group to which the joint names
correspond.
planning_time (float): Maximum allowed time for planning, in seconds.
planner_config_name (str): Type of planner to use.
plan_type (str): either pfs or rr. If rr we don't use the path length threshold.
Return:
list of list of float: A sequence of points representing the joint
configurations at each point on the path.
"""
planner_client = rospy.ServiceProxy(self.planners[planner_number], GetMotionPlan)
rospy.loginfo("%s Plan Trajectory Wrapper: got a plan_trajectory request for %s with start = %s and goal = %s" \
% (rospy.get_name(), self.planners[planner_number], start_point, goal_point))
# Put together the service request.
req = GetMotionPlanRequest()
req.motion_plan_request.workspace_parameters.header.stamp = rospy.get_rostime()
req.motion_plan_request.group_name = group_name
req.motion_plan_request.num_planning_attempts = 1
req.motion_plan_request.allowed_planning_time = planning_time
req.motion_plan_request.planner_id = planner_config_name #using RRT planner by default
req.motion_plan_request.start_state.joint_state.header.stamp = rospy.get_rostime()
req.motion_plan_request.start_state.joint_state.name = joint_names
req.motion_plan_request.start_state.joint_state.position = start_point
req.motion_plan_request.goal_constraints.append(Constraints())
req.motion_plan_request.goal_constraints[0].joint_constraints = []
for i in xrange(len(joint_names)):
temp_constraint = JointConstraint()
temp_constraint.joint_name = joint_names[i]
temp_constraint.position = goal_point[i]
temp_constraint.tolerance_above = 0.05;
temp_constraint.tolerance_below = 0.05;
req.motion_plan_request.goal_constraints[0].joint_constraints.append(temp_constraint)
#call the planner
rospy.wait_for_service(self.planners[planner_number])
rospy.loginfo("Plan Trajectory Wrapper: sent request to service %s" % planner_client.resolved_name)
plan_time = np.inf
try:
plan_time = time.time()
response = planner_client(req)
plan_time = time.time() - plan_time
except rospy.ServiceException, e:
rospy.loginfo("%s Plan Trajectory Wrapper: service call failed: %s"
% (rospy.get_name(), e))
return plan_time, None
def init_gripper(self):
"""
Initialise gripper interface.
"""
# Gripper
self.gripper_group_name = "hand"
self.gripper_joints = ["panda_finger_joint1", "panda_finger_joint2"]
self.gripper_links = ["panda_leftfinger", "panda_rightfinger"]
self.gripper_max_speed = 0.2
# Service client for IK
self.plan_gripper_path_client = self.create_client(
GetMotionPlan, "plan_kinematic_path")
while not self.plan_gripper_path_client.wait_for_service(
timeout_sec=1.0):
self.get_logger().info(
"Service [plan_kinematic_path] not currently available, waiting..."
)
self.gripper_path_request = GetMotionPlan.Request()
self.gripper_path_request.motion_plan_request.workspace_parameters.header.frame_id = \
self.arm_base_link
# self.gripper_path_request.motion_plan_request.workspace_parameters.header.stamp = \
# "Set during request"
self.gripper_path_request.motion_plan_request.workspace_parameters.min_corner.x = -0.855
self.gripper_path_request.motion_plan_request.workspace_parameters.min_corner.y = -0.855
self.gripper_path_request.motion_plan_request.workspace_parameters.min_corner.z = -0.36
self.gripper_path_request.motion_plan_request.workspace_parameters.max_corner.x = 0.855
self.gripper_path_request.motion_plan_request.workspace_parameters.max_corner.y = 0.855
self.gripper_path_request.motion_plan_request.workspace_parameters.max_corner.z = 1.19
# self.gripper_path_request.motion_plan_request.start_state = "Ignored"
self.gripper_path_request.motion_plan_request.goal_constraints = \
[Constraints()]
(self.gripper_path_request.motion_plan_request.goal_constraints[0].
joint_constraints.append(JointConstraint()))
(self.gripper_path_request.motion_plan_request.goal_constraints[0].
joint_constraints[0].joint_name) = self.gripper_joints[0]
# (self.gripper_path_request.motion_plan_request.goal_constraints[0].joint_constraints[0]
# .position) = "Set during request"
# (self.gripper_path_request.motion_plan_request.goal_constraints[0].joint_constraints[0]
# .tolerance_above) = "Ignored"
# (self.gripper_path_request.motion_plan_request.goal_constraints[0].joint_constraints[0]
# .tolerance_below) = "Ignored"
self.gripper_path_request.motion_plan_request.goal_constraints[
0].joint_constraints[0].weight = 1.0
# self.gripper_path_request.motion_plan_request.path_constraints = "Ignored"
# self.gripper_path_request.motion_plan_request.trajectory_constraints = "Ignored"
# self.gripper_path_request.motion_plan_request.reference_trajectories = "Ignored"
# self.gripper_path_request.motion_plan_request.planner_id = "Ignored"
self.gripper_path_request.motion_plan_request.group_name = self.gripper_group_name
self.gripper_path_request.motion_plan_request.num_planning_attempts = 1
self.gripper_path_request.motion_plan_request.allowed_planning_time = 0.1
# self.gripper_path_request.motion_plan_request.max_velocity_scaling_factor = \
# "Set during request"
self.gripper_path_request.motion_plan_request.max_acceleration_scaling_factor = 0.0
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
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
right_arm.set_path_constraints(consts)
# 计算夹取姿态
if obj_theta <= 0:
(Qux, Quy, Quz,
Quw) = quaternion_from_euler(90.01 / 180 * pi, 0,
(-180 - obj_theta) / 180 * pi)
else:
(Qux, Quy, Quz,
Quw) = quaternion_from_euler(90.01 / 180 * pi, 0,
(180 - obj_theta) / 180 * pi)
# (Qux, Quy, Quz, Quw) = quaternion_from_euler(90/180*pi, 0, -180/180*pi)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Qux
pose_goal.orientation.y = Quy
pose_goal.orientation.z = Quz
pose_goal.orientation.w = Quw
# pose_goal.position.y = 0.0244387395252 + (-0.595877665686-0.0244387395252)*obj_x/320
# pose_goal.position.x = 0.625989932306 + (0.197518221397-0.625989932306)*obj_y/240
pose_goal.position.x = 0.1819576873 + (160 - obj_y) * (
0.494596343128 - 0.1819576873) / 160
pose_goal.position.y = obj_x * (
-0.455644324437 + 0.0238434066464) / 220 - 0.0238434066464
pose_goal.position.z = 0.0942404372508
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 _cmd_to_request(self, robot):
"""Transforms the given command to a MotionPlanRequest."""
req = MotionPlanRequest()
# Set general info
req.planner_id = self._planner_id
req.group_name = self._planning_group
req.max_velocity_scaling_factor = self._vel_scale
req.max_acceleration_scaling_factor = self._acc_scale
req.allowed_planning_time = 1.0
# Set an empty diff as start_state => the current state is used by the planner
req.start_state.is_diff = True
# Set goal constraint
if self._goal is None:
raise NameError("Goal is not given.")
goal_constraints = Constraints()
# goal as Pose in Cartesian space
if isinstance(self._goal, Pose):
goal_pose = self._get_goal_pose(robot)
robot_reference_frame = robot._robot_commander.get_planning_frame()
goal_constraints.orientation_constraints.append(
_to_ori_constraint(goal_pose, robot_reference_frame,
self._target_link))
goal_constraints.position_constraints.append(
_to_pose_constraint(goal_pose, robot_reference_frame,
self._target_link))
# goal as list of int or float in joint space
elif isinstance(self._goal, list):
joint_names = robot._robot_commander.get_group(
self._planning_group).get_active_joints()
joint_values = self._get_joint_pose(robot)
if len(joint_names) != len(joint_values):
raise IndexError(
"Given joint goal does not match the planning group " +
req.group_name + ".")
for joint_name, joint_value in zip(joint_names, joint_values):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_name
joint_constraint.position = joint_value
joint_constraint.weight = 1
goal_constraints.joint_constraints.append(joint_constraint)
else:
raise NotImplementedError("Unknown type of goal is given.")
req.goal_constraints.append(goal_constraints)
return req
def plan_execute_pose_constraints_cb(self, goal):
feedback = PlanExecutePoseConstraintsFeedback()
result = PlanExecutePoseConstraintsResult()
result.result = True
#include default arm_base_joint // it is bug that moveit does not exclude passive joint in thier joint trajectory.
js_base = JointConstraint()
js_base.joint_name = "arm_base_joint"
js_base.position = 0.0
js_base.tolerance_above = 0.001
js_base.tolerance_below = 0.001
js_base.weight = 1.0
self.contraints.name = "constraints"
self.contraints.joint_constraints.append(js_base)
print("==================== Joint Constraints ====================")
for js in goal.joint_constraints:
self.contraints.joint_constraints.append(js)
print(js)
self.group.set_path_constraints(self.contraints)
print("============================================================")
self.group.clear_pose_targets()
self.group.set_start_state_to_current_state()
try:
self.group.set_pose_target(goal.target_pose)
except MoveItCommanderException:
result.result = False
return
rospy.loginfo('Planning goal pose...')
plan1 = self.group.plan()
if len(plan1.joint_trajectory.points) == 0:
result.result = False
return
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
display_trajectory.trajectory.append(plan1)
self.traj_publisher.publish(display_trajectory)
# rospy.sleep(0.5)
rospy.loginfo('Start moving...')
self.group.go(wait=True)
rospy.loginfo('End moving')
# rospy.sleep(2.0)
self.group.set_path_constraints(None)
self.contraints.joint_constraints = []
rospy.loginfo('Planning goal pose succeeded.')
self.action_plan_execute_pose_w_constraints.set_succeeded(result)
def set_path_constraints(self, value):
# In order to force planning in joint coordinates we add dummy joint
# constrains.
empty_joint_constraints = JointConstraint()
empty_joint_constraints.joint_name = "r1_joint_grip"
empty_joint_constraints.position = 0
empty_joint_constraints.tolerance_above = 1.5
empty_joint_constraints.tolerance_below = 1.5
empty_joint_constraints.weight = 1
value.joint_constraints.append(empty_joint_constraints)
MoveGroupCommander.set_path_constraints(self, value)
def set_global_defaults( self, group ):
"""
Initializes the proxy with default parameters to use with connections
@param group : string move group name
"""
# Default planning parameters for new connections
num_planning_attempts=5
allowed_planning_time=10.0
plan_only=True
planner_id='RRTConnectkConfigDefault'
# Set the default plan request data
plan_request = MotionPlanRequest()
plan_request.start_state.is_diff = True # Flags start state as empty to use current state on server
plan_request.num_planning_attempts = num_planning_attempts
plan_request.allowed_planning_time = allowed_planning_time
plan_request.planner_id = planner_id
plan_request.group_name = group
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._motion_plan_requests[group] = plan_request
ProxyMoveItClient._default_motion_plan_requests[group] = copy.deepcopy(plan_request)
planning_options = PlanningOptions()
planning_options.plan_only = plan_only
planning_options.planning_scene_diff.robot_state.is_diff = True # Flags start state as empty to use current state on server
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._planning_options[group] = planning_options
ProxyMoveItClient._default_planning_options[group] = copy.deepcopy(planning_options)
# Set up the default joint constraints
joint_constraints = {}
for name in ProxyMoveItClient._joint_names[group]:
joint_constraints[name] = JointConstraint(joint_name=name, tolerance_above=0.05, tolerance_below=0.05,weight=1.0)
#@TODO - retrieve from ROS parameters
ProxyMoveItClient._joint_constraints[group] = joint_constraints
ProxyMoveItClient._default_joint_constraints[group] = copy.deepcopy(joint_constraints)
# @TODO - add real constraints
ProxyMoveItClient._position_constraints[group] = []
ProxyMoveItClient._default_position_constraints[group] = []
ProxyMoveItClient._orientation_constraints[group] = []
ProxyMoveItClient._default_orientation_constraints[group] = []
ProxyMoveItClient._visibility_constraints[group] = []
ProxyMoveItClient._default_visibility_constraints[group] = []
ProxyMoveItClient._robot_states[group] = RobotState()
ProxyMoveItClient._default_robot_states[group] = RobotState()
def setJointConstraints(goal_config, group_handle): constraints = []; names = group_handle.get_joints(); for num in range(0,len(names)): jnt_constraint = JointConstraint(); jnt_constraint.joint_name = names[num]; jnt_constraint.position = goal_config[num]; jnt_constraint.tolerance_above = 0.0001; jnt_constraint.tolerance_below = 0.0001; jnt_constraint.weight = 1.0; constraints.append(jnt_constraint); return constraints;
def build_joint_constraints(joint_state):
joint_constraints = []
for i, joint_angle in enumerate(joint_state):
jc = JointConstraint()
jc.joint_name = self.JOINTS[i]
jc.position = joint_angle
jc.tolerance_above = np.radians(
self.MAX_LINEAR_JOINT_FLUCTUATION)
jc.tolerance_below = np.radians(
self.MAX_LINEAR_JOINT_FLUCTUATION)
jc.weight = 1.0
joint_constraints.append(jc)
return joint_constraints
def right_arm_go_down_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
waypoints = []
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# 添加路径起始点
waypoints.append(copy.deepcopy(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.0239
right_joint_const.weight = 1.0
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
right_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.position.z = pose_goal.position.z - 0.045
# pose_goal.position.y = pose_goal.position.y - 0.1
# 添加路径末端点
waypoints.append(copy.deepcopy(pose_goal))
# 路径规划
(plan, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "End effector pose %s" % waypoints
# robot = self.robot
# display_trajectory_publisher = self.display_trajectory_publisher
# display_trajectory = moveit_msgs.msg.DisplayTrajectory()
# display_trajectory.trajectory_start = robot.get_current_state()
# display_trajectory.trajectory.append(plan)
# display_trajectory_publisher.publish(display_trajectory)
# 规划和输出动作
right_arm.execute(plan, wait=False)
def set_constraints(self):
consts = Constraints()
consts.joint_constraints.append(
JointConstraint(joint_name='shoulder_pan_joint',
position=0.3,
tolerance_above=2.5,
tolerance_below=0.1,
weight=1))
consts.joint_constraints.append(
JointConstraint(joint_name='shoulder_lift_joint',
position=0,
tolerance_above=0.0,
tolerance_below=2.5,
weight=1))
#consts.joint_constraints.append(
# JointConstraint(joint_name='elbow_joint',
# position=0, tolerance_above=pi,
# tolerance_bel=pi, weight=0.5))
consts.joint_constraints.append(
JointConstraint(joint_name='wrist_1_joint',
position=-2.2,
tolerance_above=2.2,
tolerance_below=2.7,
weight=1))
consts.joint_constraints.append(
JointConstraint(joint_name='wrist_2_joint',
position=0.4,
tolerance_above=0.1,
tolerance_below=4.0,
weight=1))
consts.joint_constraints.append(
JointConstraint(joint_name='wrist_3_joint',
position=pi / 4.0,
tolerance_above=0.5,
tolerance_below=0.5,
weight=1))
self.group.set_path_constraints(consts)
def make_moveit_action_goal(joint_names, joint_positions):
goal_config_constraint = Constraints()
for name, position in zip(joint_names, joint_positions):
joint_constraint = JointConstraint()
joint_constraint.joint_name = name
joint_constraint.position = position
goal_config_constraint.joint_constraints.append(joint_constraint)
req = MotionPlanRequest()
req.group_name = 'both_arms'
req.goal_constraints.append(goal_config_constraint)
goal = MoveGroupGoal()
goal.request = req
return goal
