while plan[0] != True:
plan = arm_group.plan(pose_target)
if plan[0]:
traj = plan[1]
arm_group.execute(traj, wait=True)
arm_group.stop()
arm_group.clear_pose_targets()
# rospy.sleep(1)
#GO FRONT
state = RobotState()
arm_group.set_start_state(state)
pose_target.position.x -= 0.18
arm_group.set_pose_target(pose_target)
plan1 = arm_group.plan(pose_target)
while plan1[0] != True:
state = RobotState()
arm_group.set_start_state(state)
plan1 = arm_group.plan(pose_target)
if plan1[0]:
traj = plan1[1]
arm_group.execute(traj, wait=True)
arm_group.go()
arm_group.stop()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
class PlannerAnnotationParser(AnnotationParserBase):
"""
Parses the annotations files that contains the benchmarking
information.
"""
def __init__(self, path_to_annotation, path_to_data):
super(PlannerAnnotationParser, self).__init__(path_to_annotation,
path_to_data)
self.parse()
self._load_scene()
self._init_planning()
self.benchmark()
def check_results(self, results):
"""
Returns the results from the planner, checking them against any eventual validation data
(no validation data in our case).
"""
return self.planner_data
def _load_scene(self):
"""
Loads the proper scene for the planner.
It can be either a python static scene or bag containing an occupancy map.
"""
scene = self._annotations["scene"]
for element in scene:
if element["type"] == "launch":
self.play_launch(element["name"])
elif element["type"] == "python":
self.load_python(element["name"])
elif element["type"] == "bag":
self.play_bag(element["name"])
for _ in range(150):
rospy.sleep(0.3)
# wait for the scene to be spawned properly
rospy.sleep(0.5)
def _init_planning(self):
"""
Initialises the needed connections for the planning.
"""
self.group_id = self._annotations["group_id"]
self.planners = self._annotations["planners"]
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(self.group_id)
self._marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10,
latch=True)
self._planning_time_sub = rospy.Subscriber(
'/move_group/result', MoveGroupActionResult,
self._check_computation_time)
rospy.sleep(1)
self.group.set_num_planning_attempts(
self._annotations["planning_attempts"])
self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
self.group.set_planning_time(self._annotations["planning_time"])
self.group.allow_replanning(self._annotations["allow_replanning"])
self._comp_time = []
self.planner_data = []
def benchmark(self):
for test_id, test in enumerate(self._annotations["tests"]):
marker_position_1 = test["start_xyz"]
marker_position_2 = test["goal_xyz"]
self._add_markers(marker_position_1, "Start test \n sequence",
marker_position_2, "Goal")
# Start planning in a given joint position
joints = test["start_joints"]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:6] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
joints = test["goal_joints"]
for planner in self.planners:
if planner == "stomp":
planner = "STOMP"
elif planner == "sbpl":
planner = "AnytimeD*"
self.planner_id = planner
self.group.set_planner_id(planner)
self._plan_joints(
joints,
self._annotations["name"] + "-test_" + str(test_id))
return self.planner_data
def _add_markers(self, point, text1, point_2, text2):
# add marker for start and goal pose of EE
marker_array = MarkerArray()
marker_1 = Marker()
marker_1.header.frame_id = "world"
marker_1.header.stamp = rospy.Time.now()
marker_1.type = Marker.SPHERE
marker_1.scale.x = 0.04
marker_1.scale.y = 0.04
marker_1.scale.z = 0.04
marker_1.lifetime = rospy.Duration()
marker_2 = deepcopy(marker_1)
marker_1.color.g = 0.5
marker_1.color.a = 1.0
marker_1.id = 0
marker_1.pose.position.x = point[0]
marker_1.pose.position.y = point[1]
marker_1.pose.position.z = point[2]
marker_2.color.r = 0.5
marker_2.color.a = 1.0
marker_2.id = 1
marker_2.pose.position.x = point_2[0]
marker_2.pose.position.y = point_2[1]
marker_2.pose.position.z = point_2[2]
marker_3 = Marker()
marker_3.header.frame_id = "world"
marker_3.header.stamp = rospy.Time.now()
marker_3.type = Marker.TEXT_VIEW_FACING
marker_3.scale.z = 0.10
marker_3.lifetime = rospy.Duration()
marker_4 = deepcopy(marker_3)
marker_3.text = text1
marker_3.id = 2
marker_3.color.g = 0.5
marker_3.color.a = 1.0
marker_3.pose.position.x = point[0]
marker_3.pose.position.y = point[1]
marker_3.pose.position.z = point[2] + 0.15
marker_4.text = text2
marker_4.id = 3
marker_4.color.r = 0.5
marker_4.color.a = 1.0
marker_4.pose.position.x = point_2[0]
marker_4.pose.position.y = point_2[1]
marker_4.pose.position.z = point_2[2] + 0.15
marker_array.markers = [marker_1, marker_2, marker_3, marker_4]
self._marker_pub.publish(marker_array)
rospy.sleep(1)
def _plan_joints(self, joints, test_name):
# plan to joint target and determine success
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
plan_time = "N/A"
total_joint_rotation = "N/A"
comp_time = "N/A"
plan_success = self._check_plan_success(plan)
if plan_success:
plan_time = self._check_plan_time(plan)
total_joint_rotation = self._check_plan_total_rotation(plan)
while not self._comp_time:
rospy.sleep(0.5)
comp_time = self._comp_time.pop(0)
self.planner_data.append([
self.planner_id, test_name,
str(plan_success), plan_time, total_joint_rotation, comp_time
])
@staticmethod
def _check_plan_success(plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
@staticmethod
def _check_plan_time(plan):
# find duration of successful plan
number_of_points = len(plan.joint_trajectory.points)
time = plan.joint_trajectory.points[number_of_points -
1].time_from_start.to_sec()
return time
@staticmethod
def _check_plan_total_rotation(plan):
# find total joint rotation in successful trajectory
angles = [0, 0, 0, 0, 0, 0]
number_of_points = len(plan.joint_trajectory.points)
for i in range(number_of_points - 1):
angles_temp = [
abs(x - y)
for x, y in zip(plan.joint_trajectory.points[i + 1].positions,
plan.joint_trajectory.points[i].positions)
]
angles = [x + y for x, y in zip(angles, angles_temp)]
total_angle_change = sum(angles)
return total_angle_change
def _check_computation_time(self, msg):
# get computation time for successful plan to be found
if msg.status.status == 3:
self._comp_time.append(msg.result.planning_time)
def _check_path_length(self, plan):
# find distance travelled by end effector
# not yet implemented
return
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
right_arm.set_planning_time(15)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('right_arm_zero')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('right_arm_zero')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
class GraspRose(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=['succeeded', 'failed'],
input_keys=['active_arm'],
output_keys=['reset_rotation'])
# initialize tf listener
self.listener = tf.TransformListener()
### Create a handle for the Move Group Commander
self.mgc_left = MoveGroupCommander("arm_left")
self.mgc_right = MoveGroupCommander("arm_right")
### Create a handle for the Planning Scene Interface
self.psi = PlanningSceneInterface()
self.eef_step = 0.01
self.jump_threshold = 2
rospy.sleep(1)
def execute(self, userdata):
#rospy.loginfo("Grasping rose...")
#sss.wait_for_input()
if not self.plan_and_execute(userdata):
userdata.reset_rotation = False
return "failed"
return "succeeded"
def plan_and_execute(self, userdata):
# add table
ps = PoseStamped()
ps.header.frame_id = "table_top"
ps.pose.position.x = -0.05
ps.pose.position.z = 0.05
ps.pose.orientation.w = 1
filename = rospkg.RosPack().get_path(
"hmi_manipulation") + "/files/hmi_table.stl"
self.psi.add_mesh("table", ps, filename)
### Set next (virtual) start state
start_state = RobotState()
(pre_grasp_config,
error_code) = sss.compose_trajectory("arm_" + userdata.active_arm,
"pre_grasp")
if error_code != 0:
rospy.logerr("unable to parse pre_grasp configuration")
return False
start_state.joint_state.name = pre_grasp_config.joint_names
start_state.joint_state.position = pre_grasp_config.points[0].positions
start_state.is_diff = True
if userdata.active_arm == "left":
self.mgc_left.set_start_state(start_state)
elif userdata.active_arm == "right":
self.mgc_right.set_start_state(start_state)
else:
rospy.logerr("invalid arm_active")
return False
### Plan Approach
approach_pose_offset = PoseStamped()
approach_pose_offset.header.frame_id = "current_rose"
approach_pose_offset.header.stamp = rospy.Time(0)
approach_pose_offset.pose.position.x = -0.12
approach_pose_offset.pose.orientation.w = 1
try:
approach_pose = self.listener.transformPose(
"odom_combined", approach_pose_offset)
except Exception, e:
rospy.logerr("could not transform pose. Exception: %s", str(e))
return False
if userdata.active_arm == "left":
(traj_approach,
frac_approach) = self.mgc_left.compute_cartesian_path(
[approach_pose.pose], self.eef_step, self.jump_threshold,
True)
elif userdata.active_arm == "right":
(traj_approach,
frac_approach) = self.mgc_right.compute_cartesian_path(
[approach_pose.pose], self.eef_step, self.jump_threshold,
True)
else:
rospy.logerr("invalid arm_active")
return False
traj_approach = self.smooth_cartesian_path(traj_approach)
if not (frac_approach == 1.0):
rospy.logerr("Unable to plan approach trajectory")
#sss.say(["no approach trajectory: skipping rose"], False)
return False
### Set next (virtual) start state
traj_approach_endpoint = traj_approach.joint_trajectory.points[-1]
start_state = RobotState()
start_state.joint_state.name = traj_approach.joint_trajectory.joint_names
start_state.joint_state.position = traj_approach_endpoint.positions
start_state.is_diff = True
if userdata.active_arm == "left":
self.mgc_left.set_start_state(start_state)
elif userdata.active_arm == "right":
self.mgc_right.set_start_state(start_state)
else:
rospy.logerr("invalid arm_active")
return False
### Plan Grasp
grasp_pose_offset = PoseStamped()
grasp_pose_offset.header.frame_id = "current_rose"
grasp_pose_offset.header.stamp = rospy.Time(0)
grasp_pose_offset.pose.orientation.w = 1
grasp_pose = self.listener.transformPose("odom_combined",
grasp_pose_offset)
if userdata.active_arm == "left":
(traj_grasp, frac_grasp) = self.mgc_left.compute_cartesian_path(
[grasp_pose.pose], self.eef_step, self.jump_threshold, True)
elif userdata.active_arm == "right":
(traj_grasp, frac_grasp) = self.mgc_right.compute_cartesian_path(
[grasp_pose.pose], self.eef_step, self.jump_threshold, True)
else:
rospy.logerr("invalid arm_active")
return False
traj_grasp = self.smooth_cartesian_path(traj_grasp)
if not (frac_grasp == 1.0):
rospy.logerr("Unable to plan grasp trajectory")
#sss.say(["no grasp trajectory: skipping rose"], False)
return False
### Set next (virtual) start state
traj_grasp_endpoint = traj_grasp.joint_trajectory.points[-1]
start_state = RobotState()
start_state.joint_state.name = traj_grasp.joint_trajectory.joint_names
start_state.joint_state.position = traj_grasp_endpoint.positions
start_state.is_diff = True
if userdata.active_arm == "left":
self.mgc_left.set_start_state(start_state)
elif userdata.active_arm == "right":
self.mgc_right.set_start_state(start_state)
else:
rospy.logerr("invalid arm_active")
return False
### Plan Lift
lift_pose_offset = PoseStamped()
lift_pose_offset.header.frame_id = "current_rose"
lift_pose_offset.header.stamp = rospy.Time(0)
if userdata.active_arm == "left":
lift_pose_offset.pose.position.z = -0.2 #-0.3#-0.12
elif userdata.active_arm == "right":
lift_pose_offset.pose.position.z = 0.2 #0.3#0.12
else:
rospy.logerr("invalid active_arm: %s", userdata.active_arm)
sys.exit()
lift_pose_offset.pose.orientation.w = 1
lift_pose = self.listener.transformPose("odom_combined",
lift_pose_offset)
if userdata.active_arm == "left":
(traj_lift, frac_lift) = self.mgc_left.compute_cartesian_path(
[lift_pose.pose], self.eef_step, self.jump_threshold, True)
elif userdata.active_arm == "right":
(traj_lift, frac_lift) = self.mgc_right.compute_cartesian_path(
[lift_pose.pose], self.eef_step, self.jump_threshold, True)
else:
rospy.logerr("invalid arm_active")
return False
traj_lift = self.smooth_cartesian_path(traj_lift)
if not (frac_lift == 1.0):
rospy.logerr("Unable to plan lift trajectory")
#sss.say(["no lift trajectory: skipping rose"], False)
return False
"""
### Set next (virtual) start state
traj_lift_endpoint = traj_lift.joint_trajectory.points[-1]
start_state = RobotState()
start_state.joint_state.name = traj_lift.joint_trajectory.joint_names
start_state.joint_state.position = traj_lift_endpoint.positions
rose_primitive = SolidPrimitive()
rose_primitive.type = 3 #CYLINDER
rose_height = 0.4
rose_radius = 0.05
rose_primitive.dimensions.append(rose_height)
rose_primitive.dimensions.append(rose_radius)
rose_pose = Pose()
rose_pose.orientation.w = 1.0
rose_collision = CollisionObject()
rose_collision.header.frame_id = "gripper_"+userdata.active_arm+"_grasp_link"
rose_collision.id = "current_rose"
rose_collision.primitives.append(rose_primitive)
rose_collision.primitive_poses.append(rose_pose)
rose_collision.operation = 0 #ADD
rose_attached = AttachedCollisionObject()
rose_attached.link_name = "gripper_"+userdata.active_arm+"_grasp_link"
rose_attached.object = rose_collision
rose_attached.touch_links = ["gripper_"+userdata.active_arm+"_base_link", "gripper_"+userdata.active_arm+"_camera_link", "gripper_"+userdata.active_arm+"_finger_1_link", "gripper_"+userdata.active_arm+"_finger_2_link", "gripper_"+userdata.active_arm+"_grasp_link", "gripper_"+userdata.active_arm+"_palm_link"]
start_state.attached_collision_objects.append(rose_attached)
start_state.is_diff = True
if userdata.active_arm == "left":
self.mgc_left.set_start_state(start_state)
elif userdata.active_arm == "right":
self.mgc_right.set_start_state(start_state)
else:
rospy.logerr("invalid arm_active")
return False
#Plan retreat
pre_grasp_config = smi.get_goal_from_server("arm_"+userdata.active_arm, "pre_grasp")
#print pre_grasp_config
if pre_grasp_config == None:
rospy.logerr("GoalConfig not found on ParameterServer")
#sss.say(["GoalConfig not found on ParameterServer"], False)
return False
if userdata.active_arm == "left":
self.mgc_left.set_planner_id("RRTkConfigDefault")
traj_pre_grasp = self.mgc_left.plan(pre_grasp_config)
elif userdata.active_arm == "right":
self.mgc_right.set_planner_id("RRTkConfigDefault")
traj_pre_grasp = self.mgc_right.plan(pre_grasp_config)
else:
rospy.logerr("invalid arm_active")
return False
print traj_pre_grasp
if traj_pre_grasp == None:
rospy.logerr("Unable to plan pre_grasp trajectory")
#sss.say(["no pre_grasp trajectory: skipping rose"], False)
return False
"""
#if not (frac_approach == 1.0 and frac_grasp == 1.0 and frac_lift == 1.0 and not traj_pre_grasp == None):
if not (frac_approach == 1.0 and frac_grasp == 1.0
and frac_lift == 1.0):
rospy.logerr("Unable to plan whole grasping trajectory")
#sss.say(["skipping rose"], False)
return False
else:
#sss.say(["grasping rose"], False)
# fix trajectories to stop at the end
traj_approach.joint_trajectory.points[-1].velocities = [0] * 7
traj_grasp.joint_trajectory.points[-1].velocities = [0] * 7
traj_lift.joint_trajectory.points[-1].velocities = [0] * 7
# fix trajectories to be slower
speed_factor = 1
for i in range(len(traj_approach.joint_trajectory.points)):
traj_approach.joint_trajectory.points[
i].time_from_start *= speed_factor
for i in range(len(traj_grasp.joint_trajectory.points)):
traj_grasp.joint_trajectory.points[
i].time_from_start *= speed_factor
for i in range(len(traj_lift.joint_trajectory.points)):
traj_lift.joint_trajectory.points[
i].time_from_start *= speed_factor
### execute
#sss.wait_for_input()
sss.move("arm_" + userdata.active_arm, "pre_grasp")
#sss.wait_for_input()
rospy.loginfo("approach")
if userdata.active_arm == "left":
self.mgc_left.execute(traj_approach)
#handle_gripper = sss.move("gripper_" + userdata.active_arm, "open")
move_gripper("gripper_" + userdata.active_arm, "open")
#sss.wait_for_input()
rospy.loginfo("grasp")
self.mgc_left.execute(traj_grasp)
#sss.wait_for_input()
#sss.move("gripper_" + userdata.active_arm, "close")
move_gripper("gripper_" + userdata.active_arm, "close")
rospy.loginfo("lift")
self.mgc_left.execute(traj_lift)
#sss.wait_for_input()
#self.mgc_left.execute(traj_pre_grasp)
#rospy.sleep(1)
sss.move("base", "middle", mode="linear", blocking=False)
#rospy.sleep(0.5) #wait for base to move away from table
handle_arm = sss.move("arm_" + userdata.active_arm, "retreat")
elif userdata.active_arm == "right":
self.mgc_right.execute(traj_approach)
#sss.move("gripper_" + userdata.active_arm, "open")
move_gripper("gripper_" + userdata.active_arm, "open")
#sss.wait_for_input()
rospy.loginfo("grasp")
self.mgc_right.execute(traj_grasp)
#sss.wait_for_input()
#sss.move("gripper_" + userdata.active_arm, "close")
move_gripper("gripper_" + userdata.active_arm, "close")
rospy.loginfo("lift")
self.mgc_right.execute(traj_lift)
#sss.wait_for_input()
#self.mgc_right.execute(traj_pre_grasp)
#rospy.sleep(1)
sss.move("base", "middle", mode="linear", blocking=False)
#rospy.sleep(0.5) #wait for base to move away from table
handle_arm = sss.move("arm_" + userdata.active_arm, "retreat")
else:
rospy.logerr("invalid arm_active")
return False
#if handle_arm.get_error_code():
# #sss.say(["script server error"])
# rospy.logerr("script server error")
# sss.set_light("light_base","red")
#sss.wait_for_input()
return True
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since contraint planning can take a while
right_arm.set_planning_time(15)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.1)
# arm.set_max_velocity_scaling_factor(0.01)
arm.set_named_target("home")
arm.go()
def traj_pack(plan, joint_num=6, save_name=None):
traj_len = len(plan.joint_trajectory.points)
time_ls, pos_ls, vel_ls, acc_ls = [], [], [], []
# print plan.joint_trajectory.points
for point in plan.joint_trajectory.points:
time_ls.append(point.time_from_start.secs + point.time_from_start.nsecs / 1e9)
pos_ls.append(point.positions)
vel_ls.append(point.velocities)
acc_ls.append(point.accelerations)
pos, vel, acc = [], [], []
for i in range(traj_len):
pos.append(pos_ls[i][joint_num])
vel.append(vel_ls[i][joint_num])
acc.append(acc_ls[i][joint_num])
# 保存为CSV
if save_name is not None:
np.savetxt("%s.csv" % save_name, np.array([time_ls, pos, vel, acc]).transpose(), delimiter=',', header="time,pos,vel,acc", comments="")
return time_ls, pos, vel, acc
# ************************ 测试普通轨迹规划 *******************************
# arm.set_start_state_to_current_state()
# arm.set_named_target("test_1")
# plan = arm.plan()
# ************************ 测试连续路径轨迹规划（零点->中间点->零点） *******************************
# 重新计算轨迹时间
def retime_trajectory(plan, scale):
ref_state = robot.get_current_state()
retimed_plan = arm.retime_trajectory(ref_state, plan, velocity_scaling_factor=scale)
return retimed_plan
# 轨迹点拼接
def stitch_trajectory(plan_list):
new_traj = RobotTrajectory()
new_traj.joint_trajectory.joint_names = plan_list[0].joint_trajectory.joint_names
# 轨迹点拼接
new_points = []
for plan in plan_list:
new_points += list(plan.joint_trajectory.points)
new_traj.joint_trajectory.points = new_points
# 重新计算轨迹时间
new_traj = retime_trajectory(new_traj, scale=1.0)
return new_traj
# 轨迹列表
plan_list = []
# 设置初始状态
state = robot.get_current_state()
arm.set_start_state(state)
# 设置目标状态
aim_position1 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.57]
arm.set_joint_value_target(aim_position1)
plan1 = arm.plan()
plan_list.append(plan1)
# 保存轨迹
traj_pack(plan1, save_name = "trajectory1")
# 设置初始状态
state.joint_state.position = aim_position1
arm.set_start_state(state)
# 设置目标状态
aim_position2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
arm.set_joint_value_target(aim_position2)
plan2 = arm.plan()
plan_list.append(plan2)
plan2 = stitch_trajectory(plan_list) # 拼接轨迹1和2
traj_pack(plan2, save_name = "trajectory2")
# 设置初始状态
state.joint_state.position = aim_position2
arm.set_start_state(state)
# 设置目标状态
aim_position3 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.57]
arm.set_joint_value_target(aim_position3)
plan3 = arm.plan()
plan_list.append(plan3)
plan3 = stitch_trajectory(plan_list) # 拼接轨迹1、2和3
traj_pack(plan3, save_name = "trajectory3")
# 执行轨迹
arm.execute(plan3)
# exit(0)
####################################轨迹可视化#############################################
plan = plan3 # 用于可视化的轨迹
traj_len = len(plan.joint_trajectory.points)
print "traj_len:", traj_len
time_ls, pos, vel, acc = traj_pack(plan)
# 显示原始轨迹
print "\npos", pos
print "\nvel", vel
print "\nacc", acc
# plt.plot(time_ls, pos, 'ro') # 轨迹点
# plt.plot(time_ls, pos, 'k') # 连接轨迹点
# plt.show()
plt.figure(figsize=(6, 10))
plt.subplot(3,1,1)
plt.plot(time_ls, pos, 'ro')
# plt.plot(time_ls, pos, 'k')
plt.grid('on')
plt.title('Origin trajectory')
plt.xlabel('time (s)')
plt.ylabel('position (rad)')
plt.xlim(time_ls[0]-0.5, time_ls[-1]+0.5)
plt.ylim(min(pos) - 0.1, max(pos) + 0.1)
plt.subplot(3,1,2)
plt.plot(time_ls, vel, 'ro')
# plt.plot(time_ls, vel, 'k')
plt.grid('on')
plt.xlabel('time (s)')
plt.ylabel('velocity (rad / s)')
plt.xlim(time_ls[0]-0.5, time_ls[-1]+0.5)
plt.subplot(3,1,3)
plt.plot(time_ls, acc, 'ro')
# plt.plot(time_ls, acc, 'k')
plt.grid('on')
plt.xlabel('time (s)')
plt.ylabel('acceleration (rad / s$^{2}$)')
plt.xlim(time_ls[0]-0.5, time_ls[-1]+0.5)
plt.savefig('Origin trajectory.svg', dpi=600, bbox_inches='tight')
# plt.savefig('Origin trajectory.pdf', dpi=600, bbox_inches='tight')
plt.show()
# 插值测试
# Interpolation(time_ls, pos, vel, acc)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class SrFastGrasp:
def __init__(self):
self.__marker_pub = rospy.Publisher("visualization_marker",
Marker,
queue_size=1)
self.__grasp_server = rospy.Service("grasp_from_bounding_box",
GetFastGraspFromBoundingBox,
self.__bounding_box_cb)
self.__default_grasp = 'super_amazing_grasp'
self.__get_state = rospy.ServiceProxy(
'/grasp_warehouse/get_robot_state', GetState)
hand_group = rospy.get_param("~hand_group", "right_hand")
arm_group = rospy.get_param("~arm_group", "right_arm")
self.__group = MoveGroupCommander(hand_group)
self.__arm_g = MoveGroupCommander(arm_group)
self.__ik = rospy.ServiceProxy("compute_ik", GetPositionIK)
def __modify_grasp_pose(self, grasp, pose):
"""
Aligns grasp with axis from origin to center of object.
A crude way to make a vaguely sane orientation for the hand
that seems to more or less work.
"""
v1 = numpy.array(
[pose.pose.position.x, pose.pose.position.y, pose.pose.position.z])
v1_length = numpy.linalg.norm(v1)
v1 = v1 / v1_length
v2 = [1, 0, -v1[0] / v1[2]]
v2 = v2 / numpy.linalg.norm(v2)
v3 = numpy.cross(v1, v2)
v3 = v3 / numpy.linalg.norm(v3)
m = [[v3[0], v1[0], v2[0]], [v3[1], v1[1], v2[1]],
[v3[2], v1[2], v2[2]]]
q = quaternion_from_matrix(m)
grasp.grasp_pose = deepcopy(pose)
grasp.grasp_pose.pose.orientation.x = q[0]
grasp.grasp_pose.pose.orientation.y = q[1]
grasp.grasp_pose.pose.orientation.z = q[2]
grasp.grasp_pose.pose.orientation.w = q[3]
def __bounding_box_cb(self, request):
box = request.bounding_box
pose = request.pose
if SolidPrimitive.BOX != box.type:
rospy.logerr("Bounding volume must be a BOX.")
return None
self.__send_marker_to_rviz(box, pose)
grasp_name = self.__select_grasp()
grasp = self.__get_grasp(grasp_name)
self.__modify_grasp_pose(grasp, pose)
return grasp
def __select_grasp(self):
return self.__default_grasp
def __get_grasp(self, name):
try:
open_state = self.__get_state(name + "_open", "").state
closed_state = self.__get_state(name + "_closed", "").state
except Exception:
rospy.logfatal("Couldn't get grasp pose from db.")
return Grasp()
try:
self.__group.set_start_state_to_current_state()
pre_pose = self.__group.plan(open_state.joint_state)
self.__group.set_start_state(open_state)
pose = self.__group.plan(closed_state.joint_state)
except Exception:
rospy.logfatal("Couldn't plan grasp trajectories.")
return Grasp()
grasp = Grasp()
grasp.id = name
grasp.pre_grasp_posture = pre_pose.joint_trajectory
grasp.grasp_posture = pose.joint_trajectory
grasp.pre_grasp_approach.desired_distance = 0.2
grasp.pre_grasp_approach.min_distance = 0.1
grasp.pre_grasp_approach.direction.vector.x = 0
grasp.pre_grasp_approach.direction.vector.y = -1
grasp.pre_grasp_approach.direction.vector.z = 0
return grasp
def __get_major_axis(self, box):
m = max(box.dimensions)
max_index = [i for i, j in enumerate(box.dimensions) if j == m]
return max_index[-1] # Get the LAST axis with max val.
def __send_marker_to_rviz(self, box, pose):
marker = self.__get_marker_from_box(box, pose)
self.__marker_pub.publish(marker)
def __get_marker_from_box(self, box, pose):
marker = Marker()
marker.pose = pose.pose
marker.header.frame_id = pose.header.frame_id
marker.scale.x = box.dimensions[SolidPrimitive.BOX_X]
marker.scale.y = box.dimensions[SolidPrimitive.BOX_Y]
marker.scale.z = box.dimensions[SolidPrimitive.BOX_Z]
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.5
marker.lifetime = rospy.rostime.Duration()
marker.type = Marker.CUBE
marker.ns = "sr_fast_grasp_target"
marker.id = 0
marker.action = Marker.ADD
return marker
class TestPlanners(object):
def __init__(self, group_id, planner):
rospy.init_node('moveit_test_planners', anonymous=True)
self.pass_list = []
self.fail_list = []
self.planner = planner
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(group_id)
rospy.sleep(1)
self.group.set_planner_id(self.planner)
self.test_trajectories_empty_environment()
self.test_waypoints()
self.test_trajectories_with_walls_and_ground()
for pass_test in self.pass_list:
print(pass_test)
for fail_test in self.fail_list:
print(fail_test)
def _add_walls_and_ground(self):
# publish a scene
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below ground (to prevent collision with
# the robot itself)
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.1))
p.pose.position.x = 0.4
p.pose.position.y = 0.85
p.pose.position.z = 0.4
p.pose.orientation.x = 0.5
p.pose.orientation.y = -0.5
p.pose.orientation.z = 0.5
p.pose.orientation.w = 0.5
self.scene.add_box("wall_front", p, (0.8, 2, 0.01))
p.pose.position.x = 1.33
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707388
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.706825
self.scene.add_box("wall_right", p, (0.8, 2, 0.01))
p.pose.position.x = -0.5
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707107
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.707107
self.scene.add_box("wall_left", p, (0.8, 2, 0.01))
# rospy.sleep(1)
def _check_plan(self, plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
def _plan_joints(self, joints):
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
return self._check_plan(plan)
def test_trajectories_rotating_each_joint(self):
# test_joint_values = [numpy.pi/2.0, numpy.pi-0.33, -numpy.pi/2]
test_joint_values = [numpy.pi / 2.0]
joints = [0.0, 0.0, 0.0, -numpy.pi / 2.0, 0.0, 0.0]
# Joint 4th is colliding with the hand
# for joint in range(6):
for joint in [0, 1, 2, 3, 5]:
for value in test_joint_values:
joints[joint] = value
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
def test_trajectories_empty_environment(self):
# Up - Does not work with sbpl but it does with ompl
joints = [0.0, -1.99, 2.19, 0.58, 0.0, -0.79, 0.0, 0.0]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# All joints up
joints = [
-1.67232, -2.39104, 0.264862, 0.43346, 2.44148, 2.48026, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Down
joints = [
-0.000348431194526, 0.397651011661, 0.0766181197394,
-0.600353691727, -0.000441966540076, 0.12612019707, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# left
joints = [
0.146182953165, -2.6791929848, -0.602721109682, -3.00575848765,
0.146075718452, 0.00420656698366, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Front
joints = [
1.425279839, -0.110370375874, -1.52548746261, -1.50659865247,
-1.42700242769, 3.1415450794, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Behind
joints = [
1.57542451065, 3.01734161219, 2.01043257686, -1.14647092839,
0.694689321451, -0.390769365032, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Should fail because it is in self-collision
joints = [
-0.289797803762, 2.37263860495, 2.69118483159, 1.65486712181,
1.04235601797, -1.69730925867, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
def test_waypoints(self):
# Start planning in a given joint position
joints = [
-0.324590029242, -1.42602359749, -1.02523472017, -0.754761892979,
0.344227622185, -3.03250264451, 0.0, 0.0
]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:8] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
waypoints = []
initial_pose = self.group.get_current_pose().pose
initial_pose.position.x = -0.301185959729
initial_pose.position.y = 0.517069787724
initial_pose.position.z = 1.20681710541
initial_pose.orientation.x = 0.0207499700474
initial_pose.orientation.y = -0.723943002716
initial_pose.orientation.z = -0.689528413407
initial_pose.orientation.w = 0.00515118111221
# start with a specific position
waypoints.append(initial_pose)
# first move it down
wpose = geometry_msgs.msg.Pose()
wpose.orientation = waypoints[0].orientation
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z - 0.20
waypoints.append(copy.deepcopy(wpose))
# second front
wpose.position.y += 0.20
waypoints.append(copy.deepcopy(wpose))
# third side
wpose.position.x -= 0.20
waypoints.append(copy.deepcopy(wpose))
# fourth return to initial pose
wpose = waypoints[0]
waypoints.append(copy.deepcopy(wpose))
(plan3,
fraction) = self.group.compute_cartesian_path(waypoints, 0.01, 0.0)
if not self._check_plan(plan3) and fraction > 0.8:
self.fail_list.append("Failed: test_waypoints, " + self.planner)
else:
self.pass_list.append("Passed: test_waypoints, " + self.planner)
def test_trajectories_with_walls_and_ground(self):
self._add_walls_and_ground()
# Should fail to plan: Goal is in collision with the wall_front
joints = [
0.302173213174, 0.192487443763, -1.94298265002, 1.74920382275,
0.302143499777, 0.00130280337897, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Should fail to plan: Goal is in collision with the ground
joints = [
3.84825722288e-05, 0.643694953509, -1.14391175311, 1.09463824437,
0.000133883149666, -0.594498939239, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Goal close to right corner
joints = [
0.354696232081, -0.982224980654, 0.908055961723, -1.92328051116,
-1.3516255551, 2.8225061435, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed, test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
self.scene.remove_world_object("ground")
self.scene.remove_world_object("wall_left")
self.scene.remove_world_object("wall_right")
self.scene.remove_world_object("wall_front")
def set_start_state(self, msg):
self._start_state = msg
MoveGroupCommander.set_start_state(self, msg)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
initial_state.header = Header()
initial_state.header.frame_id = "base_link"
initial_state.header.stamp = rospy.Time.now()
initial_state.name = [
"joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"
]
# initial_state.position = joint_state_msg.position
initial_state.position = [
28 * (pi / 180), 34.72 * (pi / 180), 1.52 * (pi / 180),
34.86 * (pi / 180), -42.73 * (pi / 180), -27.7 * (pi / 180)
]
robot_state = RobotState()
robot_state.joint_state = initial_state
group.set_start_state(robot_state)
group.set_pose_reference_frame("base_link")
# group.set_goal_orientation_tolerance(0.5)
print("waiting for hitting point !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
target_pose = rospy.wait_for_message("/robot_target_pose", Pose)
rospy.loginfo("Goal Point Recieved Time")
# target_joint = rospy.wait_for_message("/robot_target_joint", JointState)
# #print ("hitting point recieved")
# group.set_trajectory_constraints()
plan = group.plan(target_pose)
# plan = group.plan(target_joint)
if len(plan.joint_trajectory.points) > 1:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('move_continue_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
# arm.set_goal_position_tolerance(0.01)
# arm.set_goal_orientation_tolerance(0.1)
arm.set_max_velocity_scaling_factor(0.5)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_start_state_to_current_state()
arm.set_named_target("fusion_left")
arm.go()
# 轨迹速度加速度缩放
def scale_trajectory_vel_acc(traj, vel_scale, acc_scale):
new_traj = RobotTrajectory()
new_traj.joint_trajectory = traj.joint_trajectory
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
points = list(traj.joint_trajectory.points)
for i in range(n_points):
point = JointTrajectoryPoint()
point.positions = traj.joint_trajectory.points[i].positions
point.time_from_start = traj.joint_trajectory.points[
i].time_from_start / vel_scale
point.velocities = list(
traj.joint_trajectory.points[i].velocities)
point.accelerations = list(
traj.joint_trajectory.points[i].accelerations)
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * vel_scale
point.accelerations[
j] = point.accelerations[j] * acc_scale * acc_scale
points[i] = point
new_traj.joint_trajectory.points = points
return new_traj
# 重新计算轨迹时间
def retime_trajectory(plan, scale):
ref_state = robot.get_current_state()
retimed_plan = arm.retime_trajectory(ref_state,
plan,
velocity_scaling_factor=scale)
return retimed_plan
# 拼接轨迹点
def stitch_positions(positions):
plan_list = []
state = robot.get_current_state()
# 路径规划, 连接各路径点
for i in range(len(positions)):
# 设置前一路径点为待规划的路径起点, 起始点除外
if i > 0:
state.joint_state.position = positions[i - 1]
arm.set_start_state(state)
# 设置目标状态
arm.set_joint_value_target(positions[i])
plan = arm.plan()
plan_list.append(plan)
# 创建新轨迹, 重新计算时间
new_traj = RobotTrajectory()
new_traj.joint_trajectory.joint_names = plan_list[
0].joint_trajectory.joint_names
# 轨迹点拼接
new_points = []
for plan in plan_list:
new_points += list(plan.joint_trajectory.points)
new_traj.joint_trajectory.points = new_points
# 重新计算轨迹时间
new_traj = retime_trajectory(new_traj, scale=0.5)
return new_traj
# positions = [[0.055, 0.317, -0.033, -1.292, -0.099, -0.087, -1.575],
# [1.055, 1.317, -1.033, -1.292, -0.099, -0.087, -1.575],
# [1.055, -1.317, 1.033, 1.292, -0.099, 1.087, -1.575]]
#
# plan = stitch_positions(positions)
# arm.execute(plan)
positions_name = [
'cali_3', 'fusion_left2', 'fusion_left1', 'fusion_1',
'fusion_right1', 'fusion_right2'
]
positions = []
for name in positions_name:
position_dict = arm.get_named_target_values(name)
joint_names = sorted(position_dict.keys())
position = []
for joint_name in joint_names:
position.append(position_dict[joint_name])
positions.append(position)
plan = stitch_positions(positions)
# 速度加速度缩放
# plan = scale_trajectory_vel_acc(plan, 0.25, 0.25)
# new_traj = retime_trajectory(plan, scale=0.25)
arm.execute(plan)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
exit()
# arm.get_named_target_values()
# ########################################### test #############################################
# 轨迹列表
plan_list = []
# 设置初始状态
state = robot.get_current_state()
arm.set_start_state(state)
# 设置目标状态
aim_position1 = [0.055, 0.317, -0.033, -1.292, -0.099, -0.087, -1.575]
arm.set_joint_value_target(aim_position1)
plan1 = arm.plan()
plan_list.append(plan1)
# 设置初始状态
state.joint_state.position = aim_position1
arm.set_start_state(state)
# 设置目标状态
aim_position2 = [1.055, 1.317, -1.033, -1.292, -0.099, -0.087, -1.575]
arm.set_joint_value_target(aim_position2)
plan2 = arm.plan()
plan_list.append(plan2)
# 设置初始状态
state.joint_state.position = aim_position2
arm.set_start_state(state)
# 设置目标状态
aim_position3 = [1.055, -1.317, 1.033, 1.292, -0.099, 1.087, -1.575]
arm.set_joint_value_target(aim_position3)
plan3 = arm.plan()
plan_list.append(plan3)
new_traj = RobotTrajectory()
new_traj.joint_trajectory.joint_names = plan1.joint_trajectory.joint_names
# 轨迹点拼接
new_points = []
for plan in plan_list:
new_points += list(plan.joint_trajectory.points)
new_traj.joint_trajectory.points = new_points
# 重新计算轨迹时间
new_traj = retime_trajectory(new_traj, scale=0.5)
# 执行轨迹
arm.execute(new_traj)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
"current_rose", rospy.Time(0))
goal_pose = Pose()
goal_pose.position.x = trans[0]
goal_pose.position.y = trans[1]
goal_pose.position.z = trans[2]
goal_pose.orientation.x = rot[0]
goal_pose.orientation.y = rot[1]
goal_pose.orientation.z = rot[2]
goal_pose.orientation.w = rot[3]
print goal_pose
except Exception, e:
rospy.logerr(
"could get transform from base_link to current_rose. Exception: %s",
str(e))
sys.exit()
mgc.set_start_state(start_state)
(traj_approach, frac_approach) = mgc.compute_cartesian_path([goal_pose],
0.01, 4, True)
print frac_approach
print traj_approach
### Set next (virtual) start state
traj_approach_endpoint = traj_approach.joint_trajectory.points[-1]
start_state = RobotState()
#start_state.header = traj_approach.header
#start_state.header.stamp = rospy.Time.now()
start_state.joint_state.name = traj_approach.joint_trajectory.joint_names
start_state.joint_state.position = traj_approach_endpoint.positions
start_state.is_diff = True
### Plan Lift
class SrRobotCommander(object):
"""
Base class for hand and arm commanders.
"""
def __init__(self, name):
"""
Initialize MoveGroupCommander object.
@param name - name of the MoveIt group.
"""
self._name = name
self._move_group_commander = MoveGroupCommander(name)
self._robot_commander = RobotCommander()
self._robot_name = self._robot_commander._r.get_robot_name()
self.refresh_named_targets()
self._warehouse_name_get_srv = rospy.ServiceProxy("get_robot_state",
GetState)
self._planning_scene = PlanningSceneInterface()
self._joint_states_lock = threading.Lock()
self._joint_states_listener = \
rospy.Subscriber("joint_states", JointState,
self._joint_states_callback, queue_size=1)
self._joints_position = {}
self._joints_velocity = {}
self._joints_effort = {}
self._joints_state = None
self._clients = {}
self.__plan = None
self._controllers = {}
rospy.wait_for_service('compute_ik')
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
controller_list_param = rospy.get_param("/move_group/controller_list")
# create dictionary with name of controllers and corresponding joints
self._controllers = {item["name"]: item["joints"] for item in controller_list_param}
self._set_up_action_client(self._controllers)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
threading.Thread(None, rospy.spin)
def _is_trajectory_valid(self, trajectory, required_keys):
if type(trajectory) != list:
rospy.logerr("Trajectory is not a list of waypoints")
return False
no_error = True
for k in required_keys:
if "|" in k:
optional = k.split("|")
if len(set(optional).intersection(set(trajectory[0].keys()))) == 0:
rospy.logerr("Trajectory is missing both of {} keys".format(optional))
no_error = False
else:
if k not in list(trajectory[0].keys()):
rospy.logerr("Trajectory waypoint missing {}".format(k))
no_error = False
return no_error
def set_planner_id(self, planner_id):
"""
Sets the planner_id used for all future planning requests.
@param planner_id - The string for the planner id, set to None to clear.
"""
self._move_group_commander.set_planner_id(planner_id)
def set_num_planning_attempts(self, num_planning_attempts):
self._move_group_commander.set_num_planning_attempts(num_planning_attempts)
def set_planning_time(self, seconds):
"""
Specifies the amount of time to be used for motion planning.
Some planning algorithms might require more time than others to compute
a valid solution.
"""
self._move_group_commander.set_planning_time(seconds)
def get_end_effector_pose_from_named_state(self, name):
state = self._warehouse_name_get_srv(name, self._robot_name).state
return self.get_end_effector_pose_from_state(state)
def get_end_effector_pose_from_state(self, state):
header = Header()
fk_link_names = [self._move_group_commander.get_end_effector_link()]
header.frame_id = self._move_group_commander.get_pose_reference_frame()
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0]
def get_planning_frame(self):
"""
@return - returns the name of the frame wrt which the motion planning
is computed.
"""
return self._move_group_commander.get_planning_frame()
def set_pose_reference_frame(self, reference_frame):
"""
Set the reference frame to assume for poses of end-effectors.
@param reference_frame - name of the frame.
"""
self._move_group_commander.set_pose_reference_frame(reference_frame)
def get_group_name(self):
return self._name
def refresh_named_targets(self):
self._srdf_names = self.__get_srdf_names()
self._warehouse_names = self.__get_warehouse_names()
def set_max_velocity_scaling_factor(self, value):
"""
Set a scaling factor for optionally reducing the maximum joint velocity.
@param value - Allowed values are in (0,1].
"""
self._move_group_commander.set_max_velocity_scaling_factor(value)
def set_max_acceleration_scaling_factor(self, value):
"""
Set a scaling factor for optionally reducing the maximum joint accelaration.
@param value - Allowed values are in (0,1].
"""
self._move_group_commander.set_max_acceleration_scaling_factor(value)
def allow_looking(self, value):
"""
Enable/disable looking around for motion planning.
@param value - boolean.
"""
self._move_group_commander.allow_looking(value)
def allow_replanning(self, value):
"""
Enable/disable replanning in case new obstacles are detected
while executing a plan.
@param value - boolean.
"""
self._move_group_commander.allow_replanning(value)
def execute(self):
"""
Executes the last plan made.
@return - Success of execution.
"""
is_executed = False
if self.check_plan_is_valid():
is_executed = self._move_group_commander.execute(self.__plan)
self.__plan = None
else:
rospy.logwarn("No plans were made, not executing anything.")
if not is_executed:
rospy.logerr("Execution failed.")
else:
rospy.loginfo("Execution succeeded.")
return is_executed
def execute_plan(self, plan):
"""
Executes a given plan.
@param plan - RobotTrajectory msg that contains the trajectory
to the set goal state.
@return - Success of execution.
"""
is_executed = False
if self.check_given_plan_is_valid(plan):
is_executed = self._move_group_commander.execute(plan)
self.__plan = None
else:
rospy.logwarn("Plan is not valid, not executing anything.")
if not is_executed:
rospy.logerr("Execution failed.")
else:
rospy.loginfo("Execution succeeded.")
return is_executed
def move_to_joint_value_target(self, joint_states, wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param wait - should method wait for movement end or not.
@param angle_degrees - are joint_states in degrees or not.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update((joint, radians(i))
for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self._move_group_commander.go(wait=wait)
def set_start_state_to_current_state(self):
return self._move_group_commander.set_start_state_to_current_state()
def plan_to_joint_value_target(self, joint_states, angle_degrees=False, custom_start_state=None):
"""
Set target of the robot's links and plans.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param angle_degrees - are joint_states in degrees or not.
@param custom_start_state - specify a start state different than the current state
This is a blocking method.
@return - motion plan (RobotTrajectory msg) that contains the trajectory to the set goal state.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update((joint, radians(i))
for joint, i in joint_states_cpy.items())
if custom_start_state is None:
self._move_group_commander.set_start_state_to_current_state()
else:
self._move_group_commander.set_start_state(custom_start_state)
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
return self.__plan
def check_plan_is_valid(self):
"""
Checks if current plan contains a valid trajectory
"""
return (self.__plan is not None and len(self.__plan.joint_trajectory.points) > 0)
def check_given_plan_is_valid(self, plan):
"""
Checks if given plan contains a valid trajectory
"""
return (plan is not None and len(plan.joint_trajectory.points) > 0)
def evaluate_given_plan(self, plan):
"""
Returns given plan quality calculated by a weighted sum of angles traveled by each
of the joints, giving higher weights to the joints closer to the base of the robot,
thus penalizing them as smallmovements of these joints will result in bigger movements
of the end effector. Formula:
PQ = sum_(i=0)^(n-1){w_i * abs(x_i - x_(i0)}, where:
n - number of robot's joints,
w - weight specified for each joint,
x - joint's goal position,
x_0 - joint's initial position.
The lower the value, the better the plan.
"""
if plan is None:
return None
num_of_joints = len(plan.joint_trajectory.points[0].positions)
weights = numpy.array(sorted(range(1, num_of_joints + 1), reverse=True))
plan_array = numpy.empty(shape=(len(plan.joint_trajectory.points),
num_of_joints))
for i, point in enumerate(plan.joint_trajectory.points):
plan_array[i] = point.positions
deltas = abs(numpy.diff(plan_array, axis=0))
sum_deltas = numpy.sum(deltas, axis=0)
sum_deltas_weighted = sum_deltas * weights
plan_quality = float(numpy.sum(sum_deltas_weighted))
return plan_quality
def evaluate_plan(self):
return self.evaluate_given_plan(self.__plan)
def evaluate_plan_quality(self, plan_quality, good_threshold=20, medium_threshold=50):
if plan_quality > medium_threshold:
rospy.logwarn("Low plan quality! Value: {}".format(plan_quality))
return 'poor'
elif (plan_quality > good_threshold and plan_quality < medium_threshold):
rospy.loginfo("Medium plan quality. Value: {}".format(plan_quality))
return 'medium'
elif plan_quality < good_threshold:
rospy.loginfo("Good plan quality. Value: {}".format(plan_quality))
return 'good'
def get_robot_name(self):
return self._robot_name
def named_target_in_srdf(self, name):
return name in self._srdf_names
def set_named_target(self, name):
"""
Set a joint configuration by name.
@param name - name of the target which must correspond to a name defined,
either in the srdf or in the mongo warehouse database.
@return - bool to confirm that the target has been correctly set.
"""
if name in self._srdf_names:
self._move_group_commander.set_named_target(name)
elif (name in self._warehouse_names):
response = self._warehouse_name_get_srv(name, self._robot_name)
active_names = self._move_group_commander._g.get_active_joints()
joints = response.state.joint_state.name
positions = response.state.joint_state.position
js = {}
for n, this_name in enumerate(joints):
if this_name in active_names:
js[this_name] = positions[n]
try:
self._move_group_commander.set_joint_value_target(js)
except Exception as e:
rospy.loginfo(e)
else:
rospy.logerr("Unknown named state '%s'..." % name)
return False
return True
def get_named_target_joint_values(self, name):
"""
Get the joint angles for targets specified by name.
@param name - @param name - name of the target which must correspond to a name defined,
either in the srdf or in the mongo warehouse database.
@return - joint values of the named target.
"""
output = dict()
if (name in self._srdf_names):
output = self._move_group_commander._g.get_named_target_values(str(name))
elif (name in self._warehouse_names):
js = self._warehouse_name_get_srv(
name, self._robot_name).state.joint_state
for x, n in enumerate(js.name):
if n in self._move_group_commander._g.get_joints():
output[n] = js.position[x]
else:
rospy.logerr("No target named %s" % name)
return None
return output
def get_end_effector_link(self):
return self._move_group_commander.get_end_effector_link()
def get_current_pose(self, reference_frame=None):
"""
Get the current pose of the end effector.
@param reference_frame - The desired reference frame in which end effector pose should be returned.
If none is passed, it will use the planning frame as reference.
@return - geometry_msgs.msg.Pose() - current pose of the end effector.
"""
if reference_frame is not None:
try:
trans = self.tf_buffer.lookup_transform(reference_frame,
self._move_group_commander.get_end_effector_link(),
rospy.Time(0),
rospy.Duration(5.0))
current_pose = geometry_msgs.msg.Pose()
current_pose.position.x = trans.transform.translation.x
current_pose.position.y = trans.transform.translation.y
current_pose.position.z = trans.transform.translation.z
current_pose.orientation.x = trans.transform.rotation.x
current_pose.orientation.y = trans.transform.rotation.y
current_pose.orientation.z = trans.transform.rotation.z
current_pose.orientation.w = trans.transform.rotation.w
return current_pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.logwarn("Couldn't get the pose from " + self._move_group_commander.get_end_effector_link() +
" in " + reference_frame + " reference frame")
return None
else:
return self._move_group_commander.get_current_pose().pose
def get_current_state(self):
"""
Get the current joint state of the group being used.
@return - a dictionary with the joint names as keys and current joint values.
"""
joint_names = self._move_group_commander._g.get_active_joints()
joint_values = self._move_group_commander._g.get_current_joint_values()
return dict(zip(joint_names, joint_values))
def get_current_state_bounded(self):
"""
Get the current joint state of the group being used, enforcing that they are within each joint limits.
@return - a dictionary with the joint names as keys and current joint values.
"""
current = self._move_group_commander._g.get_current_state_bounded()
names = self._move_group_commander._g.get_active_joints()
output = {n: current[n] for n in names if n in current}
return output
def get_robot_state_bounded(self):
return self._move_group_commander._g.get_current_state_bounded()
def move_to_named_target(self, name, wait=True):
"""
Set target of the robot's links and moves to it
@param name - name of the target pose defined in SRDF
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self._move_group_commander.go(wait=wait)
def plan_to_named_target(self, name, custom_start_state=None):
"""
Set target of the robot's links and plans
This is a blocking method.
@param name - name of the target pose defined in SRDF.
@param custom_start_state - specify a start state different than the current state.
@return - a motion plan (RobotTrajectory msg) that contains the trajectory to the named target.
"""
if custom_start_state is None:
self._move_group_commander.set_start_state_to_current_state()
else:
self._move_group_commander.set_start_state(custom_start_state)
if self.set_named_target(name):
self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
else:
rospy.logwarn("Could not find named target, plan not generated")
return False
return True
def __get_warehouse_names(self):
try:
list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
return list_srv("", self._robot_name).states
except rospy.ServiceException as exc:
rospy.logwarn("Couldn't access warehouse: " + str(exc))
return list()
def _reset_plan(self):
self.__plan = None
def _set_plan(self, plan):
self.__plan = plan
def __get_srdf_names(self):
return self._move_group_commander._g.get_named_targets()
def get_named_targets(self):
"""
Get the complete list of named targets, from SRDF
as well as warehouse poses if available.
@return - list of strings containing names of targets.
"""
return self._srdf_names + self._warehouse_names
def get_joints_position(self):
"""
Returns joints position.
@return - dictionary with joints positions.
"""
with self._joint_states_lock:
return self._joints_position
def get_joints_velocity(self):
"""
Returns joints velocities
@return - dictionary with joints velocities.
"""
with self._joint_states_lock:
return self._joints_velocity
def _get_joints_effort(self):
"""
Returns joints effort.
@return - dictionary with joints efforts.
"""
with self._joint_states_lock:
return self._joints_effort
def get_joints_state(self):
"""
Returns joints state
@return - JointState message
"""
with self._joint_states_lock:
return self._joints_state
def run_joint_trajectory(self, joint_trajectory):
"""
Moves robot through all joint states with specified timeouts.
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@return - Success of execution.
"""
plan = RobotTrajectory()
plan.joint_trajectory = joint_trajectory
return self._move_group_commander.execute(plan)
def make_named_trajectory(self, trajectory):
"""
Makes joint value trajectory from specified by named poses (either from
SRDF or from warehouse).
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements (n.b either name or joint_angles is required)
- name -> the name of the way point
- joint_angles -> a dict of joint names and angles
- interpolate_time -> time to move from last wp
OPTIONAL:
- pause_time -> time to wait at this wp
- degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
"""
if not self._is_trajectory_valid(trajectory, ["name|joint_angles", "interpolate_time"]):
return
current = self.get_current_state_bounded()
joint_trajectory = JointTrajectory()
joint_names = list(current.keys())
joint_trajectory.joint_names = joint_names
start = JointTrajectoryPoint()
start.positions = list(current.values())
start.time_from_start = rospy.Duration.from_sec(0.001)
joint_trajectory.points.append(start)
time_from_start = 0.0
for wp in trajectory:
joint_positions = None
if 'name' in wp.keys():
joint_positions = self.get_named_target_joint_values(wp['name'])
elif 'joint_angles' in wp.keys():
joint_positions = copy.deepcopy(wp['joint_angles'])
if 'degrees' in wp.keys() and wp['degrees']:
for joint, angle in joint_positions.items():
joint_positions[joint] = radians(angle)
if joint_positions is None:
rospy.logerr("Invalid waypoint. Must contain valid name for named target or dict of joint angles.")
return None
new_positions = {}
for n in joint_names:
new_positions[n] = joint_positions[n] if n in joint_positions else current[n]
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = [new_positions[n] for n in joint_names]
current = new_positions
time_from_start += wp['interpolate_time']
trajectory_point.time_from_start = rospy.Duration.from_sec(time_from_start)
joint_trajectory.points.append(trajectory_point)
if 'pause_time' in wp and wp['pause_time'] > 0:
extra = JointTrajectoryPoint()
extra.positions = trajectory_point.positions
time_from_start += wp['pause_time']
extra.time_from_start = rospy.Duration.from_sec(time_from_start)
joint_trajectory.points.append(extra)
return joint_trajectory
def send_stop_trajectory_unsafe(self):
"""
Sends a trajectory of all active joints at their current position.
This stops the robot.
"""
current = self.get_current_state_bounded()
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = list(current.values())
trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)
trajectory = JointTrajectory()
trajectory.points.append(trajectory_point)
trajectory.joint_names = list(current.keys())
self.run_joint_trajectory_unsafe(trajectory)
def run_named_trajectory_unsafe(self, trajectory, wait=False):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory directly via contoller.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
OPTIONAL:
- pause_time -> time to wait at this wp
"""
if self._is_trajectory_valid(trajectory, ['name|joint_angles', 'interpolate_time']):
joint_trajectory = self.make_named_trajectory(trajectory)
self.run_joint_trajectory_unsafe(joint_trajectory, wait)
def run_named_trajectory(self, trajectory):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory via moveit.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
OPTIONAL:
- pause_time -> time to wait at this wp
"""
if self._is_trajectory_valid(trajectory, ['name|joint_angles', 'interpolate_time']):
joint_trajectory = self.make_named_trajectory(trajectory)
self.run_joint_trajectory(joint_trajectory)
def move_to_position_target(self, xyz, end_effector_link="", wait=True):
"""
Specify a target position for the end-effector and moves to it
preserving the current orientation of the end-effector.
@param xyz - new position of end-effector.
@param end_effector_link - name of the end effector link.
@param wait - should method wait for movement end or not.
"""
pose = self._move_group_commander.get_current_pose()
pose.pose.position.x = xyz[0]
pose.pose.position.y = xyz[1]
pose.pose.position.z = xyz[2]
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_position_target(self, xyz, end_effector_link="", custom_start_state=None):
"""
Specify a target position for the end-effector and plans preserving the current orientation of end-effector.
This is a blocking method.
@param xyz - new position of end-effector.
@param end_effector_link - name of the end effector link.
@param custom_start_state - specify a start state different than the current state.
"""
pose = self._move_group_commander.get_current_pose()
pose.pose.position.x = xyz[0]
pose.pose.position.y = xyz[1]
pose.pose.position.z = xyz[2]
if custom_start_state is None:
self._move_group_commander.set_start_state_to_current_state()
else:
self._move_group_commander.set_start_state(custom_start_state)
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
return self.__plan
def move_to_pose_target(self, pose, end_effector_link="", wait=True):
"""
Specify a target pose for the end-effector and moves to it
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw].
@param end_effector_link - name of the end effector link.
@param wait - should method wait for movement end or not.
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_pose_target(self, pose, end_effector_link="", alternative_method=False, custom_start_state=None):
"""
Specify a target pose for the end-effector and plans.
This is a blocking method.
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw].
@param end_effector_link - name of the end effector link.
@param alternative_method - use set_joint_value_target instead of set_pose_target.
@param custom_start_state - specify a start state different than the current state.
"""
if custom_start_state is None:
self._move_group_commander.set_start_state_to_current_state()
else:
self._move_group_commander.set_start_state(custom_start_state)
if alternative_method:
self._move_group_commander.set_joint_value_target(pose, end_effector_link)
else:
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()[CONST_TUPLE_TRAJECTORY_INDEX]
return self.__plan
def _joint_states_callback(self, joint_state):
"""
The callback function for the topic joint_states.
It will store the received joint position, velocity and efforts
information into dictionaries.
@param joint_state - the message containing the joints data.
"""
with self._joint_states_lock:
self._joints_state = joint_state
self._joints_position = {n: p for n, p in
zip(joint_state.name,
joint_state.position)}
self._joints_velocity = {n: v for n, v in
zip(joint_state.name,
joint_state.velocity)}
self._joints_effort = {n: v for n, v in
zip(joint_state.name, joint_state.effort)}
def _set_up_action_client(self, controller_list):
"""
Sets up an action client to communicate with the trajectory controller.
"""
self._action_running = {}
for controller_name in controller_list.keys():
self._action_running[controller_name] = False
service_name = controller_name + "/follow_joint_trajectory"
self._clients[controller_name] = SimpleActionClient(service_name,
FollowJointTrajectoryAction)
if self._clients[controller_name].wait_for_server(timeout=rospy.Duration(4)) is False:
err_msg = 'Failed to connect to action server ({}) in 4 sec'.format(service_name)
rospy.logwarn(err_msg)
def move_to_joint_value_target_unsafe(self, joint_states, time=0.002,
wait=True, angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller).
@param wait - should method wait for movement end or not.
@param angle_degrees - are joint_states in degrees or not.
"""
# self._update_default_trajectory()
# self._set_targets_to_default_trajectory(joint_states)
goals = {}
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update((joint, radians(i))
for joint, i in joint_states_cpy.items())
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = []
point = JointTrajectoryPoint()
point.positions = []
for x in joint_states_cpy.keys():
if x in controller_joints:
goal.trajectory.joint_names.append(x)
point.positions.append(joint_states_cpy[x])
point.time_from_start = rospy.Duration.from_sec(time)
goal.trajectory.points = [point]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def action_is_running(self, controller=None):
if controller is not None:
return self._action_running[controller]
for controller_running in self._action_running.values():
if controller_running:
return True
return False
def _action_done_cb(self, controller, terminal_state, result):
self._action_running[controller] = False
def _call_action(self, goals):
for client in self._clients:
self._action_running[client] = True
self._clients[client].send_goal(
goals[client], lambda terminal_state, result: self._action_done_cb(client, terminal_state, result))
def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
"""
Moves robot through all joint states with specified timeouts.
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@param wait - should method wait for movement end or not.
"""
goals = {}
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory = copy.deepcopy(joint_trajectory)
indices_of_joints_in_this_controller = []
for i, joint in enumerate(joint_trajectory.joint_names):
if joint in controller_joints:
indices_of_joints_in_this_controller.append(i)
goal.trajectory.joint_names = [
joint_trajectory.joint_names[i] for i in indices_of_joints_in_this_controller]
for point in goal.trajectory.points:
if point.positions:
point.positions = [point.positions[i] for i in indices_of_joints_in_this_controller]
if point.velocities:
point.velocities = [point.velocities[i] for i in indices_of_joints_in_this_controller]
if point.effort:
point.effort = [point.effort[i] for i in indices_of_joints_in_this_controller]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def plan_to_waypoints_target(self, waypoints, reference_frame=None,
eef_step=0.005, jump_threshold=0.0, custom_start_state=None):
"""
Specify a set of waypoints for the end-effector and plans.
This is a blocking method.
@param reference_frame - the reference frame in which the waypoints are given.
@param waypoints - an array of poses of end-effector.
@param eef_step - configurations are computed for every eef_step meters.
@param jump_threshold - maximum distance in configuration space between consecutive points in the
resulting path.
@param custom_start_state - specify a start state different than the current state.
@return - motion plan (RobotTrajectory msg) that contains the trajectory to the set wayapoints targets.
"""
if custom_start_state is None:
self._move_group_commander.set_start_state_to_current_state()
else:
self._move_group_commander.set_start_state(custom_start_state)
old_frame = self._move_group_commander.get_pose_reference_frame()
if reference_frame is not None:
self.set_pose_reference_frame(reference_frame)
self.__plan, fraction = self._move_group_commander.compute_cartesian_path(waypoints, eef_step, jump_threshold)
self.set_pose_reference_frame(old_frame)
return self.__plan, fraction
def set_teach_mode(self, teach):
"""
Activates/deactivates the teach mode for the robot.
Activation: stops the the trajectory controllers for the robot, and
sets it to teach mode.
Deactivation: stops the teach mode and starts trajectory controllers
for the robot.
Currently this method blocks for a few seconds when called on a hand,
while the hand parameters are reloaded.
@param teach - bool to activate or deactivate teach mode
"""
if teach:
mode = RobotTeachModeRequest.TEACH_MODE
else:
mode = RobotTeachModeRequest.TRAJECTORY_MODE
self.change_teach_mode(mode, self._name)
def move_to_trajectory_start(self, trajectory, wait=True):
"""
Make and execute a plan from the current state to the first state in an pre-existing trajectory.
@param trajectory - moveit_msgs/JointTrajectory.
@param wait - Bool to specify if movement should block untill finished.
"""
if len(trajectory.points) <= 0:
rospy.logerr("Trajectory has no points in it, can't reverse...")
return None
first_point = trajectory.points[0]
end_state = dict(zip(trajectory.joint_names, first_point.positions))
self.move_to_joint_value_target(end_state, wait=wait)
@staticmethod
def change_teach_mode(mode, robot):
teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)
req = RobotTeachModeRequest()
req.teach_mode = mode
req.robot = robot
try:
resp = teach_mode_client(req)
if resp.result == RobotTeachModeResponse.ERROR:
rospy.logerr("Failed to change robot %s to mode %d", robot,
mode)
else:
rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
mode, resp.result)
except rospy.ServiceException:
rospy.logerr("Failed to call service teach_mode")
def get_ik(self, target_pose, avoid_collisions=False, joint_states=None, ik_constraints=None):
"""
Computes the inverse kinematics for a given pose. It returns a JointState.
@param target_pose - A given pose of type PoseStamped.
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false.
@param joint_states - initial joint configuration of type JointState from which the IK solution is computed.
If set to None, the current joint state is retrieved automatically.
@param ik_constraints - Set constraints of type Constraints for computing the IK solution.
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link()
service_request.pose_stamped = target_pose
service_request.timeout.secs = 1
service_request.avoid_collisions = avoid_collisions
if ik_constraints is not None:
service_request.constraints = ik_constraints
if joint_states is None:
service_request.robot_state.joint_state = self.get_joints_state()
else:
service_request.robot_state.joint_state = joint_states
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" % resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
def move_to_pose_value_target_unsafe(self, target_pose, avoid_collisions=False,
time=0.002, wait=True, ik_constraints=None):
"""
Specify a target pose for the end-effector and moves to it.
@param target_pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw].
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller).
@param wait - should method wait for movement end or not.
@param ik_constraints - Set constraints of type Constraints for computing the IK solution.
"""
joint_state = self.get_ik(target_pose, avoid_collisions, ik_constraints=ik_constraints)
if joint_state is not None:
active_joints = self._move_group_commander.get_active_joints()
current_indices = [i for i, x in enumerate(joint_state.name) if any(thing in x for thing in active_joints)]
current_names = [joint_state.name[i] for i in current_indices]
current_positions = [joint_state.position[i] for i in current_indices]
state_as_dict = dict(zip(current_names, current_positions))
self.move_to_joint_value_target_unsafe(state_as_dict, time=time, wait=wait)
