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 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 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 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 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 create_move_group_joints_goal(joint_names, joint_values, group="right_arm_torso", plan_only=True):
""" 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 = 1
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 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 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 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 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 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 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 plan_trajectory(self, start_point, goal_point, planner_number, joint_names, group_name, planning_time, planner_config_name):
"""
Given a start and goal point, returns the planned path.
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.
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("Plan Trajectory Wrapper: got a plan_trajectory request for %s with start = %s and goal = %s" % (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)
try:
response = planner_client(req)
except rospy.ServiceException, e:
rospy.loginfo("Plan Trajectory Wrapper: service call failed: %s"%e)
return None
def createJointConstraints(pose_from_params, tolerances=0.1):
"""Create a JointConstraints message with its joint names and positions with some default tolerances
@param pose_from_params dictionary with names of the joints and it's values
@param tolerances the tolerance in radians for the joint positions, defaults to 0.1
@return moveit_msgs/JointConstraints[] message with the joint names and positions"""
joint_constraints = []
for joint in pose_from_params:
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint
joint_constraint.position = pose_from_params[joint]
joint_constraint.tolerance_above = tolerances
joint_constraint.tolerance_below = tolerances
joint_constraint.weight = 1.0
joint_constraints.append(joint_constraint)
return joint_constraints
def init_path_constraints(joint_no, upper_limit, lower_limit, weight):
joint_constraint = JointConstraint()
UpperArmPath.path_constraints.name = UpperArmPath.__GROUP_NAME
upper_limit = upper_limit / 180.0 * math.pi
lower_limit = lower_limit / 180.0 * math.pi
joint_constraint.position = (upper_limit + lower_limit) / 2.0
joint_constraint.tolerance_above = (upper_limit - lower_limit) / 2.0
joint_constraint.tolerance_below = (upper_limit - lower_limit) / 2.0
joint_constraint.weight = weight
joint_constraint.joint_name = "upper_joint%d" % (joint_no)
UpperArmPath.path_constraints.joint_constraints.append(
joint_constraint)
def createJointConstraintsZero():
joint_positions = [1.7567944054, 1.68571255762, -0.35731008621,
1.06480870567, 1.36986326531, -0.662985424101,
-1.31376998814]
joint_constraints = []
for joint_num in range(1,8):
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_right_' + str(joint_num) + '_joint'
joint_constraint.tolerance_above = 0.1
joint_constraint.tolerance_below = 0.1
joint_constraint.weight = 1.0
joint_constraint.position = 0.0
joint_constraints.append(joint_constraint)
print str(joint_constraint)
return joint_constraints
def createJointConstraintsZero():
joint_positions = [
1.7567944054, 1.68571255762, -0.35731008621, 1.06480870567,
1.36986326531, -0.662985424101, -1.31376998814
]
joint_constraints = []
for joint_num in range(1, 8):
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_right_' + str(joint_num) + '_joint'
joint_constraint.tolerance_above = 0.1
joint_constraint.tolerance_below = 0.1
joint_constraint.weight = 1.0
joint_constraint.position = 0.0
joint_constraints.append(joint_constraint)
print str(joint_constraint)
return joint_constraints
def createJointConstraints(joint_names, joint_positions, tolerances=0.1):
"""Create a JointConstraints message with its joint names and positions with some default tolerances
@param joint_names names of the joints which will reference to values in param joint_positions
@param joint_positions values for the joints specified in joint_names
@param tolerances the tolerance in radians for the joint positions, defaults to 0.1
@return moveit_msgs/JointConstraints[] message with the joint names and positions"""
joint_constraints = []
for joint, pos in zip(joint_names, joint_positions):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint
joint_constraint.position = pos
joint_constraint.tolerance_above = tolerances
joint_constraint.tolerance_below = tolerances
joint_constraint.weight = 1.0
joint_constraints.append(joint_constraint)
return joint_constraints
def plan_execute_named_pose_cb(self, goal):
feedback = PlanExecuteNamedPoseFeedback()
result = PlanExecuteNamedPoseResult()
result.result = True
self.group.clear_pose_targets()
#self.group.set_start_state_to_current_state()
#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)
self.group.set_path_constraints(self.contraints)
try:
self.group.set_named_target(goal.target_name)
except MoveItCommanderException:
result.result = False
self.action_plan_execute_named_pose.set_succeeded(result)
return
rospy.loginfo('Planning named [%s] pose...' % goal.target_name)
plan1 = self.group.plan()
# print("plan1 type : ", type(plan1))
# print("plan : ", plan1)
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('Planning named pose succeeded.')
self.action_plan_execute_named_pose.set_succeeded(result)
def compute_ik(self, pose):
if isinstance(pose, rox.Transform):
pose = rox_msg.transform2pose_stamped_msg(pose)
assert isinstance(pose, PoseStamped)
if pose.header.frame_id is None or pose.header.frame_id == "":
pose.header.frame_id = "world"
get_planning_scene_req = GetPlanningSceneRequest()
get_planning_scene_req.components.components = PlanningSceneComponents.ROBOT_STATE
get_planning_scene_res = self._get_planning_scene(
get_planning_scene_req)
robot_state_start = get_planning_scene_res.scene.robot_state
ik_request = PositionIKRequest()
ik_request.group_name = self.moveit_group.get_name()
ik_request.robot_state = robot_state_start
ik_request.avoid_collisions = True
ik_request.timeout = rospy.Duration(30.0)
ik_request.pose_stamped = pose
ik_request.ik_link_name = self.moveit_group.get_end_effector_link()
for i in xrange(len(robot_state_start.joint_state.name)):
j = JointConstraint()
j.joint_name = robot_state_start.joint_state.name[i]
j.position = robot_state_start.joint_state.position[i]
j.tolerance_above = np.deg2rad(170)
j.tolerance_below = np.deg2rad(170)
j.weight = 100
ik_request.constraints.joint_constraints.append(j)
ik_request_req = GetPositionIKRequest(ik_request)
ik_request_res = self._compute_ik(ik_request_req)
if (ik_request_res.error_code.val != MoveItErrorCodes.SUCCESS):
raise Exception("Could not compute inverse kinematics")
return ik_request_res.solution.joint_state.position
def plan_between_states(group_name,
joint_state_A,
joint_state_B,
robot_state=None):
"""
Plan trajectory between two joint states.
"""
# Create robot state
robot_state = joint_to_robot_state(joint_state_A, robot_state)
# Prepare motion plan request
req = GetMotionPlanRequest()
req.motion_plan_request.start_state = robot_state
req.motion_plan_request.group_name = group_name
req.motion_plan_request.planner_id = "RRTConnectkConfigDefault"
constraints = Constraints()
constraints.name = "goal"
for name, pos in zip(joint_state_B.name, joint_state_B.position):
joint_constraints = JointConstraint()
joint_constraints.joint_name = name
joint_constraints.position = pos
joint_constraints.tolerance_above = 0.001
joint_constraints.tolerance_below = 0.001
joint_constraints.weight = 100
constraints.joint_constraints.append(joint_constraints)
req.motion_plan_request.goal_constraints.append(constraints)
# Call planner
planner_srv = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan)
try:
res = planner_srv(req)
except rospy.service.ServiceException, e:
rospy.logerr("Service 'plan_kinematic_path' call failed: %s", str(e))
return None
def set_joint_goal(self,
joint_positions,
tolerance=0.001,
weight=1.0,
joint_names=None):
"""
Set goal position in joint space. With `joint_names` specified, `joint_positions` can be
defined for specific joints. Otherwise, first `n` joints defined in `init_robot()` will be
used, where `n` is the length of `joint_positions`.
"""
if joint_names == None:
joint_names = self.arm_joints
for i in range(len(joint_positions)):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[i]
joint_constraint.position = joint_positions[i]
joint_constraint.tolerance_above = tolerance
joint_constraint.tolerance_below = tolerance
joint_constraint.weight = weight
(self.kinematic_path_request.motion_plan_request.
goal_constraints[-1].joint_constraints.append(joint_constraint))
def create_basic_mp_joint_request(joint_names, joint_values, planner_id):
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = move_group
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = 5.0
motion_plan_request.workspace_parameters = WorkspaceParameters()
motion_plan_request.max_velocity_scaling_factor = 0.5
motion_plan_request.max_acceleration_scaling_factor = 0.5
motion_plan_request.planner_id = planner_id
joint_constraints = []
for i in range(len(joint_names)):
joint_con = JointConstraint()
joint_con.joint_name = joint_names[i]
joint_con.position = joint_values[i]
joint_con.tolerance_above = 0.0001
joint_con.tolerance_below = 0.0001
joint_con.weight = 0.0001
joint_constraints.append(joint_con)
constraint = Constraints()
constraint.joint_constraints = joint_constraints
constraints = [constraint]
motion_plan_request.goal_constraints = constraints
return motion_plan_request
def move_to_state(self, joint_states):
if self.verbose:
print 'Moving to next state'
mpr = MotionPlanRequest()
con = Constraints()
for name, val in joint_states.items():
if name not in self.joints_to_exclude:
jc = JointConstraint()
jc.joint_name = name
jc.position = val
jc.tolerance_above = self.joint_angle_tolerance
jc.tolerance_below = self.joint_angle_tolerance
jc.weight = 1.0
con.joint_constraints.append(jc)
mpr.goal_constraints = [con]
mpr.group_name = self.planning_group_name
mpr.allowed_planning_time = 3.0 # [s]
# print mpr
try:
req = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan)
res = req(mpr)
traj = res.motion_plan_response.trajectory
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def kinect_planner():
global fresh_data
global joint_target, pose_target, goal
global joints_req, pose_req
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('kinect_trajectory_planner', anonymous=True)
rate = rospy.Rate(0.5)
#rospy.sleep(3)
# Instantiate a RobotCommander object.
robot = moveit_commander.RobotCommander()
# Instantiate a PlanningSceneInterface object (interface to the world surrounding the robot).
scene = moveit_commander.PlanningSceneInterface()
print "Remember to disable firewall if it's not working"
# Instantiate MoveGroupCommander objects for arms and Kinect2.
group = moveit_commander.MoveGroupCommander("Kinect2_Target")
group_left_arm = moveit_commander.MoveGroupCommander("left_arm")
group_right_arm = moveit_commander.MoveGroupCommander("right_arm")
# We create this DisplayTrajectory publisher to publish
# trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher('planned_path',
moveit_msgs.msg.DisplayTrajectory, queue_size=5)
# Set the planner for Moveit
group.set_planner_id("RRTConnectkConfigDefault")
group.set_planning_time(8)
group.set_pose_reference_frame('base_footprint')
# Setting tolerance
group.set_goal_tolerance(0.08)
group.set_num_planning_attempts(10)
# Suscribing to the desired pose topic
target = rospy.Subscriber("desired_pose", PoseStamped, callback)
target_joint = rospy.Subscriber("desired_joints", pose_w_joints, callback_joints)
# Locating the arms and the Kinect2 sensor
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
#group_kinect_values = group_kinect.get_current_joint_values()
neck_init_joints = group.get_current_joint_values()
neck_init_joints[0] = -1.346
neck_init_joints[1] = -1.116
neck_init_joints[2] = -2.121
neck_init_joints[3] = 0.830
neck_init_joints[4] = 1.490
neck_init_joints[5] = 0.050
neck_init_joints[6] = 0
neck_init_joints[7] = 0
neck_init_joints[8] = 0
neck_init_joints[9] = 0
# Creating a box to limit the arm position
box_pose = PoseStamped()
box_pose.pose.orientation.w = 1
box_pose.pose.position.x = 0.6
box_pose.pose.position.y = 0.03
box_pose.pose.position.z = 1.5
box_pose.header.frame_id = 'base_footprint'
scene.add_box('box1', box_pose, (0.4, 0.4, 0.1))
rospy.sleep(2)
# Defining position constraints for the trajectory of the kinect
neck_const = Constraints()
neck_const.name = 'neck_constraints'
target_const = JointConstraint()
target_const.joint_name = "neck_joint_end"
target_const.position = 0.95
target_const.tolerance_above = 0.45
target_const.tolerance_below = 0.05
target_const.weight = 0.9 # Importance of this constraint
neck_const.joint_constraints.append(target_const)
# Talking to the robot
client = actionlib.SimpleActionClient('/Kinect2_Target_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
print "====== Waiting for server..."
client.wait_for_server()
print "====== Connected to server"
while not rospy.is_shutdown():
if fresh_data == True: #If a new pose is received, plan.
# Update arms position
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
# Set the target pose (or joint values) and generate a plan
if pose_req:
group.set_pose_target(pose_target)
if joints_req:
group.set_joint_value_target(joint_target)
print "=========== Calculating trajectory... \n"
# Generate several plans and compare them to get the shorter one
plan_opt = dict()
differ = dict()
try:
num = 1
rep = 0
# Generate several plans and compare them to get the shortest one
#for num in range(1,8):
while num < 7:
num += 1
plan_temp = group.plan()
move(plan_temp)
plan_opt[num] = goal.trajectory
diff=0
for point in goal.trajectory.points:
# calculate the distance between initial pose and planned one
for i in range(0,6):
diff = abs(neck_init_joints[i] - point.positions[i])+abs(diff)
differ[num] = diff
# If the current plan is good, take it
if diff < 110:
break
# If plan is too bad, don't consider it
if diff > 400:
num = num - 1
print "Plan is too long. Replanning."
rep = rep + 1
if rep > 4:
num = num +1
# If no plan was found...
if differ == {}:
print "---- Fail: No motion plan found. No execution attempted. Probably robot joints are out of range."
break
else:
# Select the shortest plan
min_plan = min(differ.itervalues())
select = [k for k, value in differ.iteritems() if value == min_plan]
goal.trajectory = plan_opt[select[0]]
#print " Plan difference:======= ", differ
#print " Selected plan:========= ", select[0]
# Remove the last 4 names and data from each point (dummy joints) before sending the goal
goal.trajectory.joint_names = goal.trajectory.joint_names[:6]
for point in goal.trajectory.points:
point.positions = point.positions[:6]
point.velocities = point.velocities[:6]
point.accelerations = point.accelerations[:6]
print "Sending goal"
client.send_goal(goal)
print "Waiting for result"
client.wait_for_result()
# Change the position of the virtual joint to avoid collision
neck_joints = group.get_current_joint_values()
#neck_joints[6] = 0.7
#group.set_joint_value_target(neck_joints)
#group.go(wait=True)
except (KeyboardInterrupt, SystemExit):
client.cancel_goal()
raise
rate.sleep()
fresh_data = False
rate.sleep()
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 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 main():
"""
Main Script
"""
#===================================================
# Code to add gripper
# rospy.init_node('gripper_test')
# # Set up the right gripper
# right_gripper = robot_gripper.Gripper('right_gripper')
# #Calibrate the gripper (other commands won't work unless you do this first)
# print('Calibrating...')
# right_gripper.calibrate()
# rospy.sleep(2.0)
# #Close the right gripper to hold publisher
# # MIGHT NOT NEED THIS
# print('Closing...')
# right_gripper.close()
# rospy.sleep(1.0)
#===================================================
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = -1.1
pose.pose.position.y = 0
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1, 1, 5], "left_side_wall", pose)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = 0
pose.pose.position.y = -1.0
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1, 1, 5], "right_side_wall", pose)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = 0
pose.pose.position.y = 1.1
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1, 1, 5], "back_wall", pose)
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "ar_marker_1"
#orien_const.orientation.y = np.pi/2
#orien_const.orientation.w = 1;
orien_const.absolute_x_axis_tolerance = 1
orien_const.absolute_y_axis_tolerance = 1
orien_const.absolute_z_axis_tolerance = 1
orien_const.weight = .5
joint_const = JointConstraint()
# Constrain the position of a joint to be within a certain bound
joint_const.joint_name = "right_j6"
joint_const.position = 1.7314052734375
# the bound to be achieved is [position - tolerance_below, position + tolerance_above]
joint_const.tolerance_above = .2
joint_const.tolerance_below = .2
# A weighting factor for this constraint (denotes relative importance to other constraints. Closer to zero means less important)
joint_const.weight = .5
#for stage 2
# orien_const1 = OrientationConstraint()
# orien_const1.link_name = "right_gripper";
# orien_const1.header.frame_id = "ar_marker_1";
# #orien_const.orientation.y = np.pi/2
# #orien_const.orientation.w = 1;
# orien_const1.absolute_x_axis_tolerance = 0.5;
# orien_const1.absolute_y_axis_tolerance = 0.5;
# orien_const1.absolute_z_axis_tolerance = 0.5;
# orien_const1.weight = 1.0;
#rospy.sleep(1.0)
# while not rospy.is_shutdown():
# while not rospy.is_shutdown():
try:
#right_gripper_tip
goal_1 = PoseStamped()
goal_1.header.frame_id = "ar_marker_1"
#x, y, and z position
goal_1.pose.position.x = -0.28
goal_1.pose.position.y = -0.05
#removing the 6th layer
goal_1.pose.position.z = .38 #0.0825
# z was .1
#Orientation as a quaternion
goal_1.pose.orientation.x = 0
goal_1.pose.orientation.y = 0
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 1
plan = planner.plan_to_pose_joint(goal_1, [joint_const])
if not planner.execute_plan(plan):
raise Exception("Execution failed0")
except Exception as e:
print e
def inverse_kinematics():
# Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 50
request.ik_request.pose_stamped.header.frame_id = "base"
# Create joint constraints
#This is joint constraint will need to be set at the group
constraints = Constraints()
joint_constr = JointConstraint()
joint_constr.joint_name = "right_j0"
joint_constr.position = TORSO_POSITION
joint_constr.tolerance_above = TOLERANCE
joint_constr.tolerance_below = TOLERANCE
joint_constr.weight = 0.5
constraints.joint_constraints.append(joint_constr)
# Get the transformed AR Tag (x,y,z) coordinates
# Only care about the x coordinate of AR tag; tells use
# how far away wall is
# x,y, z tell us the origin of the AR Tag
x_coord = board_x # DONT CHANGE
y_coord = bounding_points[0]
z_coord = bounding_points[1]
y_width = bounding_points[2]
z_height = bounding_points[3]
#Creating Path Planning
waypoints = []
z_bais = 0
print(
"OMMMMMMMMMMMMMMMMMMMMMMMMMGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGG!!!!!!!!!!!!!!"
)
print(bounding_points)
for i in range(int(float(z_height) / .03)):
target_pose = Pose()
target_pose.position.x = float(x_coord - PLANNING_BIAS)
if i % 2 == 0:
#Starting positions
target_pose.position.y = float(y_coord)
else:
target_pose.position.y = y_coord + float(y_width)
#Starting positions
target_pose.position.z = float(z_coord + z_bais)
target_pose.orientation.y = 1.0 / 2**(1 / 2.0)
target_pose.orientation.w = 1.0 / 2**(1 / 2.0)
waypoints.append(target_pose)
z_bais += .03
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose = target_pose
try:
#Send the request to the service
response = compute_ik(request)
group = MoveGroupCommander("right_arm")
group.set_max_velocity_scaling_factor(0.75)
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose = target_pose
#Creating a Robot Trajectory for the Path Planning
jump_thres = 0.0
eef_step = 0.1
path, fraction = group.compute_cartesian_path([target_pose],
eef_step, jump_thres)
print("Path fraction: {}".format(fraction))
#Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
#Setting the Joint constraint
group.set_path_constraints(constraints)
if fraction < 0.5:
group.go()
else:
group.execute(path)
if i < int(float(z_height) / 0.03) and i > 0:
target2 = target_pose
target2.position.z += 0.03
path, fraction = group.compute_cartesian_path([target2],
eef_step,
jump_thres)
group.set_path_constraints(constraints)
group.execute(path)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
workspace_parameters.header.frame_id = "/BASE"
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 = "/BASE"
start_state.joint_state.name = ["j1", "j2", "j3", "j4", "j5", "flange"]
start_state.joint_state.position = [-0.2569046038066598, -0.8442722962923348, 1.849082034218144, 0.26825374068443164, -0.04090683809444329, 5.745512865657193]
start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
jc2 = JointConstraint()
jc2.joint_name = "j1"
jc2.position = -1.37353344093
jc2.tolerance_above = 0.0001
jc2.tolerance_below = 0.0001
jc2.weight = 1.0
jc3 = JointConstraint()
jc3.joint_name = "j2"
jc3.position = -1.45013378543
jc3.tolerance_above = 0.0001
jc3.tolerance_below = 0.0001
jc3.weight = 1.0
jc4 = JointConstraint()
jc4.joint_name = "j3"
jc4.position = 2.18173030842
jc4.tolerance_above = 0.0001
jc4.tolerance_below = 0.0001
jc4.weight = 1.0
def place(self, object_name, position, distance = 0.1):
if not self.is_inside_workspace(position.x, position.y, position.z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
rospy.loginfo('Stop placing')
return
pose = Pose()
pose.position = position
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), numpy.deg2rad(180.0))
pose.orientation = Quaternion(*q)
pose.position.z += distance
# Add contraint
constraint = Constraints()
joints = self.get_joints_value()
joint_constraint = JointConstraint()
joint_constraint.joint_name = "wrist_2_joint"
joint_constraint.position = joints[4]
joint_constraint.tolerance_above = 0.8
joint_constraint.tolerance_below = 0.8
joint_constraint.weight = 0.2
constraint.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = "wrist_3_joint"
joint_constraint.position = joints[5]
joint_constraint.tolerance_above = 0.8
joint_constraint.tolerance_below = 0.8
joint_constraint.weight = 0.2
constraint.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = "wrist_1_joint"
joint_constraint.position = joints[3]
joint_constraint.tolerance_above = 1.0
joint_constraint.tolerance_below = 1.0
joint_constraint.weight = 0.1
constraint.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = "elbow_joint"
joint_constraint.position = joints[2]
joint_constraint.tolerance_above = 1.2
joint_constraint.tolerance_below = 1.2
joint_constraint.weight = 0.1
constraint.joint_constraints.append(joint_constraint)
orientation_constraint = OrientationConstraint()
now = rospy.Time.now()
orientation_constraint.header.stamp = now
orientation_constraint.header.frame_id = "base_link"
orientation_constraint.link_name = "ee_link"
orientation_constraint.orientation = pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 10
orientation_constraint.absolute_y_axis_tolerance = 1.0
orientation_constraint.absolute_z_axis_tolerance = 10
orientation_constraint.weight = 0.4
constraint.orientation_constraints.append(orientation_constraint)
self.arm.set_path_constraints(constraint)
# self.arm.set_goal_position_tolerance(0.015)
rospy.loginfo('Start placing')
# self.send_message('Start placing')
self.arm.set_max_velocity_scaling_factor(0.2)
self.arm.set_workspace([-0.8, -0.8, -0.2, 0.8, 0.8, 0.3])
self.move_pose_arm(pose)
rospy.sleep(1)
self.arm.clear_path_constraints()
# self.arm.set_goal_tolerance(0.01)
# move down
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose.position.z -= distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
# place
self.gripper_release()
self.arm.detach_object(object_name)
self.clean_scene(object_name)
rospy.sleep(0.5)
# move up
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose.position.z += distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
rospy.loginfo('Place finished')
# self.send_message('Place finished')
self.arm.set_workspace([-1, -1, -0.2, 1, 1, 1.0])
def kinect_planner():
global fresh_data
global joint_target, pose_target, goal
global joints_req, pose_req
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('kinect_trajectory_planner', anonymous=True)
rate = rospy.Rate(0.5)
#rospy.sleep(3)
# Instantiate a RobotCommander object.
robot = moveit_commander.RobotCommander()
# Instantiate a PlanningSceneInterface object (interface to the world surrounding the robot).
scene = moveit_commander.PlanningSceneInterface()
print "Remember to disable firewall if it's not working"
# Instantiate MoveGroupCommander objects for arms and Kinect2.
group = moveit_commander.MoveGroupCommander("Kinect2_Target")
group_left_arm = moveit_commander.MoveGroupCommander("left_arm")
group_right_arm = moveit_commander.MoveGroupCommander("right_arm")
# We create this DisplayTrajectory publisher to publish
# trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher('planned_path',
moveit_msgs.msg.DisplayTrajectory, queue_size=5)
# Set the planner for Moveit
group.set_planner_id("RRTConnectkConfigDefault")
group.set_planning_time(8)
group.set_pose_reference_frame('base_footprint')
# Setting tolerance
group.set_goal_tolerance(0.08)
group.set_num_planning_attempts(10)
# Suscribing to the desired pose topic
target = rospy.Subscriber("desired_pose", PoseStamped, callback)
target_joint = rospy.Subscriber("desired_joints", pose_w_joints, callback_joints)
# Locating the arms and the Kinect2 sensor
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
#group_kinect_values = group_kinect.get_current_joint_values()
neck_init_joints = group.get_current_joint_values()
neck_init_joints[0] = -1.346
neck_init_joints[1] = -1.116
neck_init_joints[2] = -2.121
neck_init_joints[3] = 0.830
neck_init_joints[4] = 1.490
neck_init_joints[5] = 0.050
neck_init_joints[6] = 0
neck_init_joints[7] = 0
neck_init_joints[8] = 0
neck_init_joints[9] = 0
# Creating a box to limit the arm position
box_pose = PoseStamped()
box_pose.pose.orientation.w = 1
box_pose.pose.position.x = 0.6
box_pose.pose.position.y = 0.03
box_pose.pose.position.z = 1.5
box_pose.header.frame_id = 'base_footprint'
scene.add_box('box1', box_pose, (0.4, 0.4, 0.1))
rospy.sleep(2)
# Defining position constraints for the trajectory of the kinect
neck_const = Constraints()
neck_const.name = 'neck_constraints'
target_const = JointConstraint()
target_const.joint_name = "neck_joint_end"
target_const.position = 0.95
target_const.tolerance_above = 0.45
target_const.tolerance_below = 0.05
target_const.weight = 0.9 # Importance of this constraint
neck_const.joint_constraints.append(target_const)
# Talking to the robot
#client = actionlib.SimpleActionClient('/Kinect2_Target_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
client = actionlib.SimpleActionClient('/neck/follow_joint_trajectory', FollowJointTrajectoryAction)
print "====== Waiting for server..."
client.wait_for_server()
print "====== Connected to server"
while not rospy.is_shutdown():
print('.')
skip_iter = False
if fresh_data == True: #If a new pose is received, plan.
# Update arms position
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
# Set the target pose (or joint values) and generate a plan
if pose_req:
group.set_pose_target(pose_target)
if joints_req:
print('about to set joint target')
print(joint_target)
try:
if len(joint_target) == 6:
#Setting also values for the four virtual joints (one prismatic, and three rotational)
new_joint_target = joint_target + [0.1]*4
else:
new_joint_target = joint_target
print('setting: '),
print new_joint_target
group.set_joint_value_target(new_joint_target)
except:
print('cannot set joint target')
skip_iter = True
print "=========== Calculating trajectory... \n"
# Generate several plans and compare them to get the shorter one
plan_opt = dict()
differ = dict()
if not skip_iter:
try:
num = 1
rep = 0
# Generate several plans and compare them to get the shortest one
#for num in range(1,8):
while num < 7:
num += 1
plan_temp = group.plan()
move(plan_temp)
plan_opt[num] = goal.trajectory
diff=0
for point in goal.trajectory.points:
# calculate the distance between initial pose and planned one
for i in range(0,6):
diff = abs(neck_init_joints[i] - point.positions[i])+abs(diff)
differ[num] = diff
# If the current plan is good, take it
if diff < 110:
break
# If plan is too bad, don't consider it
if diff > 400:
num = num - 1
print "Plan is too long. Replanning."
rep = rep + 1
if rep > 4:
num = num +1
# If no plan was found...
if differ == {}:
print "---- Fail: No motion plan found. No execution attempted. Probably robot joints are out of range."
break
else:
# Select the shortest plan
min_plan = min(differ.itervalues())
select = [k for k, value in differ.iteritems() if value == min_plan]
goal.trajectory = plan_opt[select[0]]
#print " Plan difference:======= ", differ
#print " Selected plan:========= ", select[0]
# Remove the last 4 names and data from each point (dummy joints) before sending the goal
goal.trajectory.joint_names = goal.trajectory.joint_names[:6]
for point in goal.trajectory.points:
point.positions = point.positions[:6]
point.velocities = point.velocities[:6]
point.accelerations = point.accelerations[:6]
print "Sending goal"
client.send_goal(goal)
print "Waiting for result"
client.wait_for_result()
# Change the position of the virtual joint to avoid collision
neck_joints = group.get_current_joint_values()
print('neck joints:')
print neck_joints
#neck_joints[6] = 0.7
#group.set_joint_value_target(neck_joints)
#group.go(wait=True)
except (KeyboardInterrupt, SystemExit):
client.cancel_goal()
raise
rate.sleep()
fresh_data = False
rate.sleep()
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
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
drs = DisplayRobotState()
drs.state = rs
rs_pub.publish(drs)
rospy.loginfo("Published solution with False collision avoidance robot state")
rospy.sleep(5.0)
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:]
print("Joint angles...? Are coming one at a time. Give them to me in radians")
target_joint_angles = []
for joint in joint_names:
str = raw_input(joint + "\n-->")
val = float(str)
target_joint_angles.append(val)
print("I think that you want me to go to...")
print(target_joint_angles)
raw_input("Press any key to continue")
# 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 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
bagel = raw_input('Please enter in 15 joint angles and press enter: ')
bagel = bagel.split()
i = 0
for x in bagel:
bagel[i] = float(x)
i += 1
arm_joint_names = joint_names[1:]
target_joint_angles = bagel
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 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]
print 'Enter start state:'
try:
for i in range(len(joint_names)):
val = float(raw_input(joint_names[i]+':'))
goal.request.start_state.joint_state.position[i] = val
except:
print 'Using defaults for remaining joints'
print goal.request.start_state.joint_state.position
#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]
print 'Enter goal state:'
try:
for i in range(len(target_joint_angles)):
val = float(raw_input(joint_names[i]+':'))
target_joint_angles[i] = val
except:
print 'Using defaults for remaining joints'
print target_joint_angles
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())
