class PythonTimeParameterizationTest(unittest.TestCase): PLANNING_GROUP = "arm" @classmethod def setUpClass(self): self.commander = RobotCommander("robot_description") self.group = self.commander.get_group(self.PLANNING_GROUP) @classmethod def tearDown(self): pass def plan(self): start_pose = self.group.get_current_pose().pose goal_pose = self.group.get_current_pose().pose goal_pose.position.z -= 0.1 (plan, fraction) = self.group.compute_cartesian_path([start_pose, goal_pose], 0.005, 0.0) self.assertEqual(fraction, 1.0, "Cartesian path plan failed") return plan def time_parameterization(self, plan): ref_state = self.commander.get_current_state() retimed_plan = self.group.retime_trajectory( ref_state, plan, velocity_scaling_factor=0.1) return retimed_plan def test_plan_and_time_parameterization(self): plan = self.plan() retimed_plan = self.time_parameterization(plan)
def get_current_state(): # Prepare a new state object for validity check rs = RobotState() robot = RobotCommander() robot_state = robot.get_current_state() rs.joint_state.name = robot_state.joint_state.name rs.joint_state.position = list(robot_state.joint_state.position) # filler for rest of the joint angles not found in waypoint return rs
class PythonTimeParameterizationTest(unittest.TestCase): PLANNING_GROUP = "manipulator" @classmethod def setUpClass(self): self.commander = RobotCommander("robot_description") self.group = self.commander.get_group(self.PLANNING_GROUP) @classmethod def tearDown(self): pass def plan(self): start_pose = self.group.get_current_pose().pose goal_pose = self.group.get_current_pose().pose goal_pose.position.z -= 0.1 (plan, fraction) = self.group.compute_cartesian_path( [start_pose, goal_pose], 0.005, 0.0 ) self.assertEqual(fraction, 1.0, "Cartesian path plan failed") return plan def time_parameterization(self, plan, algorithm): ref_state = self.commander.get_current_state() retimed_plan = self.group.retime_trajectory( ref_state, plan, velocity_scaling_factor=0.1, acceleration_scaling_factor=0.1, algorithm=algorithm, ) return retimed_plan def test_plan_and_time_parameterization(self): plan = self.plan() retimed_plan = self.time_parameterization( plan, "iterative_time_parameterization" ) self.assertTrue( len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid" ) retimed_plan = self.time_parameterization( plan, "iterative_spline_parameterization" ) self.assertTrue( len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid" ) retimed_plan = self.time_parameterization( plan, "time_optimal_trajectory_generation" ) self.assertTrue( len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid" ) retimed_plan = self.time_parameterization(plan, "") self.assertTrue( len(retimed_plan.joint_trajectory.points) == 0, "Invalid retime algorithm" )
class ClopemaRobotCommander(MoveGroupCommander): """ Common interface to CloPeMa robor, it extends the MoveGroupCommander """ def __init__(self, group_name='arms'): """ Initialize the robot commander for particular group.""" MoveGroupCommander.__init__(self, group_name) self.set_planner_id("RRTConnectkConfigDefault") # Sleep in order to load an ik module rospy.sleep(1) self.robot = RobotCommander() self._ik_use_tip_link = True self._start_state = None self.overwrite_time_parameterization = True # Decrease the joint tolerance self.set_goal_joint_tolerance(0.00001) @staticmethod def set_servo_power_off(force=False): """Shut off the servo power.""" set_drive_power(power=False) @staticmethod def set_robot_speed(speed): """ Set robot speed, possible value can be from 0 to 1. This value is then converted to robot speed units (i.e. speed*10000). Note that there is a speed limit defined by the I001 variable on the teach pendant. This limit is in robot speed units and will overide the speed set by this function. """ set_robot_speed(speed) @staticmethod def get_robot_speed(): res = set_robot_speed(-1) return res.current_speed @staticmethod def set_robot_io(address, value): raise NotImplementedError() @staticmethod def get_robot_io(address): raise NotImplementedError() @staticmethod def get_dx100_joints(): return DX100_JOINTS @staticmethod def set_gripper_state(gripper_action, state, wait=False): """ Set gripper state i.e. open or close. :arg gripper: Gripper id, use robot.R1_GRIPPER or robot.R2_GRIPPER :arg state: Gripper state, use robot.GRIPPER_OPEN or robot.GRIPPER_CLOSE :arg wait: Wait for finish, True or False, False is depfault. Note: This function requires the node to be initialize i.e. rospy.init_node() to ba called, otherwise it will result in error. """ # Gripper action server client client = actionlib.SimpleActionClient(gripper_action, GripperCommandAction) client.wait_for_server() # Execute goal goal = GripperCommandGoal() goal.command.position = state client.send_goal(goal) if wait: client.wait_for_result(rospy.Duration.from_sec(5.0)) def get_current_state(self): """ Get a RobotState message describing the current state of the robot""" return self.robot.get_current_state() def append_ext_axis(self, start_state, trajectory, position): """Append trajectory with movement of the external axis. INPUT start_state [moveit_msgs/RobotState] trajectory [moveit_msgs/RobotTrajectory] position [Float] OUTPUT trajectory [moveit_msgs/RobotTrajectory or None] """ traj = copy.deepcopy(trajectory) N = len(traj.joint_trajectory.points) start = start_state.joint_state.position[ start_state.joint_state.name.index("ext_axis")] step = (position - start) / (N - 1) if "ext_axis" in traj.joint_trajectory.joint_names: index = traj.joint_trajectory.joint_names.index("ext_axis") for i in range(0, N): traj.joint_trajectory.points[i].positions = list( traj.joint_trajectory.points[i].positions )[index] = start + step * i traj.joint_trajectory.points[i].velocities = list( traj.joint_trajectory.points[i].velocities)[index] = 0 traj.joint_trajectory.points[i].accelerations = list( traj.joint_trajectory.points[i].accelerations)[index] = 0 else: traj.joint_trajectory.joint_names = list( traj.joint_trajectory.joint_names) + ["ext_axis"] for i in range(0, N): traj.joint_trajectory.points[i].positions = list( traj.joint_trajectory.points[i].positions) + [ start + step * i ] traj.joint_trajectory.points[i].velocities = list( traj.joint_trajectory.points[i].velocities) + [0] traj.joint_trajectory.points[i].accelerations = list( traj.joint_trajectory.points[i].accelerations) + [0] if services.check_trajectory(start_state, traj, persistent=True): return traj else: rospy.logerr( "The trajectory is not collision free after adding external axis movement." ) return None return traj def get_ik(self, pose=None, link=None, poses=None, links=None, robot_state=None, constraints=None): """Get inverse kinematic for one or two arms of the active group. Arguments: pose : {PoseStamped} Single target pose. poses : {Iterable of PoseStaped} Target poses for multiple end effectors. link : {String} Single end effector link. links : {Iterable of strings} Multiple end effector links. robot_state : {RobotState message} Initial robot state, if ommited current robot state is used. constraints : {Constrains message} Constrains for iverse kinematic solver. Note that either pose and link or poses and links must be defined. Returns: robot_state : {RobotState message} Solution to the inverse kinematic problem. """ from moveit_msgs.srv import GetPositionIK, GetPositionIKRequest from std_msgs.msg import Header # Check validity of input assert ((pose is not None and link is not None) or (poses is not None and links is not None)) if poses is None: poses = [pose] links = [link] # Get IK service proxy rospy.wait_for_service(SRV_GET_IK) srv = rospy.ServiceProxy(SRV_GET_IK, GetPositionIK) # Get robot state if neccesary if robot_state is None: robot_state = self.get_current_state() for p, l in zip(poses, links): if type(p) is Pose: p = PoseStamped(Header(0, rospy.Time.now(), "base_link"), p) # Convert to tip link if required if self._ik_use_tip_link: p_, l = self._fix_link(p.pose, l) p.pose = p_ # Pepare request req = GetPositionIKRequest() req.ik_request.group_name = GROUP_FOR_EE[l] # Allways avoid collisions req.ik_request.avoid_collisions = True req.ik_request.robot_state = robot_state if constraints is not None: req.ik_request.constraints = constraints req.ik_request.ik_link_name = l req.ik_request.pose_stamped = p res = srv(req) if res.error_code.val is res.error_code.SUCCESS: robot_state = res.solution else: rospy.logwarn("Unable to compute IK error code: %d", res.error_code.val) return None return robot_state @staticmethod def get_group_for_ee(ee_name): """Ger name of the group for end effector name.""" return GROUP_FOR_EE[ee_name] def _convert_link(self, pose, source_link, target_link): """Convert link to another link. Note: The transformation between source and target link should be fixed, but it is not checked. Arguments: pose -- Pose to be transformed. source_link -- Link of the pose. target_link -- Link where the pose should be tranformed. Returns: Pose for the target_link so that the source_link is in the orignal pose. """ Ts = pose2affine(self.robot.get_link(source_link).pose().pose) Tt = pose2affine(self.robot.get_link(target_link).pose().pose) Tp = pose2affine(pose) T = np.dot(np.linalg.inv(Tt), Ts) p = affine2pose(np.dot(Tp, np.linalg.inv(T))) return p def _fix_link(self, pose, link): """Helper to fix link if other than tip link. Arguments: pose -- Pose for given link link -- Link Returns: pose, link -- Fixed pose and link """ target_link = link if link.startswith("r1"): target_link = "r1_tip_link" elif link.startswith("r2"): target_link = "r2_tip_link" elif link.startswith("xtion1"): target_link = "xtion1_link_ee" elif link.startswith("xtion2"): target_link = "xtion2_link_ee" if link is target_link: return pose, link else: return self._convert_link(pose, link, target_link), target_link def set_start_state(self, msg): self._start_state = msg MoveGroupCommander.set_start_state(self, msg) def set_start_state_to_current_state(self): self._start_state = None MoveGroupCommander.set_start_state_to_current_state(self) def execute(self, msg): if self.overwrite_time_parameterization: start_state = self._start_state if self._start_state else self.get_current_state( ) group = self.get_name() msg = add_time_parametrisation(msg, start_state, group) if not msg.success: msg = None raise Exception("Unsucesfull time parametrisation.") else: msg = msg.parametrized_trajectory elif msg.joint_trajectory.points[-1].time_from_start == 0: raise Exception("The time from start is not set!") self._start_state = None MoveGroupCommander.execute(self, msg) def async_execute(self, msg): if self.overwrite_time_parameterization: start_state = self._start_state if self._start_state else self.get_current_state( ) group = self.get_name() msg = add_time_parametrisation(msg, start_state, group) if not msg.success: msg = None else: msg = msg.parametrized_trajectory elif msg.joint_trajectory.points[-1].time_from_start == 0: raise Exception("The time from start is not set!") self._start_state = None services.execute(msg, wait=False, persistent=True) def go(self, joints=None, wait=True): self._start_state = None MoveGroupCommander.go(self, joints, wait) def _get_start_state(self): if self._start_state is None: return self.get_current_state() else: return self._start_state def set_joint_value_target(self, arg1, arg2=None, arg3=None): """HotFix: See MoveGroupCommander for info.""" if (type(arg1) is PoseStamped) or (type(arg1) is Pose): approx = False eef = "" if arg2 is not None: if type(arg2) is str: eef = arg2 else: if type(arg2) is bool: approx = arg2 else: raise MoveItCommanderException("Unexpected type") if arg3 is not None: if type(arg3) is str: eef = arg3 else: if type(arg3) is bool: approx = arg3 else: raise MoveItCommanderException("Unexpected type") # There is a problem that when the r1_ee or r2_ee are specified as # eef the end effector ends up 0.1 m below the target position to # fix this we first transform the pose to the tip_link and then call # MoveGroupCommander if self._ik_use_tip_link: if type(arg1) is PoseStamped: p, eef = self._fix_link(arg1.pose, eef) arg1.pose = p else: arg1, eef = self._fix_link(arg1, eef) MoveGroupCommander.set_joint_value_target(self, arg1, eef, approx) else: MoveGroupCommander.set_joint_value_target(self, arg1, arg2, arg3) def compute_cartesian_path(self, waypoints, eefs, eef_step=0.01, jump_threshold=1.2, avoid_collisions=True): """Compute cartesian path for one or two arms. Arguments: waypoints -- List of poses or list tuples eefs -- End effector link or tuple of links eef_step -- jump_threshold -- """ # Wait for service an make proxi if type(eefs) is tuple and len(eefs) > 1: rospy.wait_for_service(SRV_CARTESIAN_PATH_DUAL) srv = rospy.ServiceProxy(SRV_CARTESIAN_PATH_DUAL, GetCartesianPathDual) req = GetCartesianPathDualRequest() req.start_state = self._get_start_state() # req.robot_state = self._get_start_state() req.group_name = self.get_name() req.header.frame_id = self.get_pose_reference_frame() req.header.stamp = rospy.Time.now() waypoints1, waypoints2 = zip(*waypoints) req.waypoints_1 = waypoints1 req.link_name_1 = eefs[0] req.link_name_2 = eefs[1] req.waypoints_2 = waypoints2 req.max_step = eef_step req.jump_threshold = jump_threshold req.avoid_collisions = avoid_collisions if not avoid_collisions: rospy.logwarn("!!!!Collisions are disabled!!!!") res = srv.call(req) if res.error_code.val is res.error_code.SUCCESS: return (res.solution, res.fraction) else: rospy.logwarn( "Unable to compute collsion free cartesian path! error code: %d", res.error_code.val) return None else: waypoints2, links = zip( *[self._fix_link(wp, eefs) for wp in waypoints]) rospy.wait_for_service(SRV_CARTESIAN_PATH_DUAL) srv = rospy.ServiceProxy(SRV_CARTESIAN_PATH_DUAL, GetCartesianPathDual) req = GetCartesianPathDualRequest() req.start_state = self._get_start_state() # req.robot_state = self._get_start_state() req.group_name = self.get_name() req.header.frame_id = self.get_pose_reference_frame() req.header.stamp = rospy.Time.now() req.waypoints_1 = waypoints2 req.link_name_1 = links[0] req.link_name_2 = links[0] req.waypoints_2 = waypoints2 req.max_step = eef_step req.jump_threshold = jump_threshold req.avoid_collisions = avoid_collisions if not avoid_collisions: rospy.logwarn("!!!!Collisions are disabled!!!!") res = srv.call(req) if res.error_code.val is res.error_code.SUCCESS: print res.error_code.val, req.group_name, req.header.frame_id return (res.solution, res.fraction) else: rospy.logwarn( "Unable to compute collsion free cartesian path! error code: %d", res.error_code.val) return None # return MoveGroupCommander.compute_cartesian_path(self, waypoints2, eef_step, jump_threshold, True) def plan_between_joint_states(self, js1, js2): """Plan path between two joint states. Arguments: js1,js2 : {JointState message} Returns: trajectory : {RobotTrajectory message} """ tmp_robot_state = self._get_start_state() rs = RobotState(copy.deepcopy(tmp_robot_state)) rs.update_from_joint_state(js1) self.set_start_state(rs.msg) self.set_joint_value_target(js2) traj = self.plan() self.set_start_state(tmp_robot_state) if len(traj.joint_trajectory.joint_names) <= 0: return None else: return traj def display_planned_path(self, path): """Sends the trajectory to apropriate topic to be displayed. Arguments: path : {RobotTrajectory message} Trajectory to be displayed """ self.pub = rospy.Publisher(TOPIC_DISPLAY_PLANNED_PATH, DisplayTrajectory) dsp = DisplayTrajectory() dsp.trajectory = [path] self.pub.publish(dsp) 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 grasp_from_table_plan(self, poses, tip="r1_ee", table_desk="t3_desk", final_poses=[], base_frame="base_link", offset_minus=0.02, offset_plus=0.02, table_minus=0, table_plus=0.1, grasping_angle=math.pi / 6): """ Grasp from table. Arguments: poses= [pos x, pos y, pos z, orien x, orien y orien z orien w] tip = "r1_ee" table_desk="t3_desk" final_poses= optinoal base_frame="base_link" offset_minus = 0.02 offset_plus = 0.02 table_minus = 0 table_plus = 0.1 grasping_angle = math.pi/ 6 Returns: trajectory """ rospy.wait_for_service(SRV_GRASP_FROM_TABLE) srv = rospy.ServiceProxy(SRV_GRASP_FROM_TABLE, ClopemaGraspFromTable) req = ClopemaGraspFromTableRequest() req.frame_id = base_frame req.ik_link = tip req.table_desk = table_desk req.poses = poses req.offset_minus = offset_minus req.offset_plus = offset_plus req.offset_table_minus = table_minus req.offset_table_plus = table_plus req.grasping_angle = grasping_angle req.final_poses = final_poses tmp_robot_state = self._get_start_state() self.set_start_state(tmp_robot_state) res = srv.call(req) if res.error is "": return (res.joint_trajectories) else: rospy.logwarn("Unable to compute path! error code: %d", res.error) return None def grasp_from_table_dual_plan(self, poses_1, poses_2, tip1="r1_ee", tip2="r2_ee", table_desk="t3_desk", final_poses_1=[], final_poses_2=[], base_frame="base_link", offset_minus=0.02, offset_plus=0.02, offset_table_minus=0, offset_table_plus=0.1, grasping_angle=math.pi / 6): """ Grasp from table dual. Arguments: poses_1, poses_2= [pos x, pos y, pos z, orien x, orien y orien z orien w] tip1 = "r1_ee" tip2= "r2_ee" table_desk="t3_desk" final_poses_1, final_poses_2= optinoal base_frame="base_link" offset_minus = 0.02 offset_plus = 0.02 table_minus = 0 table_plus = 0.1 grasping_angle = math.pi/ 6 Returns: trajectory """ rospy.wait_for_service(SRV_GRASP_FROM_TABLE_DUAL) srv = rospy.ServiceProxy(SRV_GRASP_FROM_TABLE_DUAL, ClopemaGraspFromTableDual) req = ClopemaGraspFromTableDualRequest() req.frame_id = base_frame req.ik_link_1 = tip1 req.ik_link_2 = tip2 req.table_desk = table_desk req.poses_1 = poses_1 req.poses_2 = poses_2 req.offset_minus = offset_minus req.offset_plus = offset_plus req.offset_table_minus = offset_table_minus req.offset_table_plus = offset_table_plus req.grasping_angle = grasping_angle req.final_poses_1 = final_poses_1 req.final_poses_2 = final_poses_2 tmp_robot_state = self._get_start_state() self.set_start_state(tmp_robot_state) res = srv.call(req) if res.error is "": return (res.joint_trajectories) else: rospy.logwarn("Unable to compute path! error code: %s", res.error) return None
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)
def main(): rospy.init_node(sys.argv[1]) total_envs = 10 total_targets = int( sys.argv[2]) #total number of collision free targets to save limb = baxter_interface.Limb('right') def_angles = limb.joint_angles() limb.set_joint_position_speed(0.65) #robot_state_collision_pub = rospy.Publisher('/robot_collision_state', DisplayRobotState, queue_size=0) #scene = PlanningSceneInterface() #scene._scene_pub = rospy.Publisher('planning_scene', # PlanningScene, # queue_size=0) robot = RobotCommander() group = MoveGroupCommander("right_arm") rs_man = RobotState() sv = StateValidity() #set_environment(robot, scene) masterModifier = ShelfSceneModifier() #sceneModifier = PlanningSceneModifier(masterModifier.obstacles) #sceneModifier.setup_scene(scene, robot, group) robot_state = robot.get_current_state() print(robot_state) rs_man = RobotState() # constructed manually for comparison rs_man.joint_state.name = robot_state.joint_state.name filler_robot_state = list(robot_state.joint_state.position) done = False masterEnvDict = {} envFileName = "testEnvironments_test1.pkl" x_bounds = [[0.89, 1.13], [ -0.2, 1.13 ]] #0 is the right half of the right table, 1 is the right table y_bounds = [[-0.66, 0], [-0.9, -0.66]] x_ranges = [ max(x_bounds[0]) - min(x_bounds[0]), max(x_bounds[1]) - min(x_bounds[1]) ] y_ranges = [ max(y_bounds[0]) - min(y_bounds[0]), max(y_bounds[1]) - min(y_bounds[1]) ] xy_bounds_dict = { 'x': x_bounds, 'y': y_bounds, 'x_r': x_ranges, 'y_r': y_ranges } iter_num = 0 #sceneModifier.delete_obstacles() while (not rospy.is_shutdown() and not done): for i_env in range(total_envs): new_pose = masterModifier.permute_obstacles() key_name = 'TrainEnv_' + str(i_env) #sceneModifier.permute_obstacles(new_pose) masterEnvDict[key_name] = {} masterEnvDict[key_name]['ObsPoses'] = {} masterEnvDict[key_name]['Targets'] = {} masterEnvDict[key_name]['Targets']['Pose'] = [] masterEnvDict[key_name]['Targets']['Joints'] = [] for i_target in range(total_targets): pose = sample_from_boundary(xy_bounds_dict) check_pose = group.get_random_pose() check_pose.pose.position.x = pose[0] check_pose.pose.position.y = pose[1] check_pose.pose.position.z = pose[2] joint_angles = ik_test(check_pose)[1] filler_robot_state[10:17] = moveit_scrambler( joint_angles.values()) rs_man.joint_state.position = tuple(filler_robot_state) while (ik_test(check_pose)[0] or not sv.getStateValidity(rs_man, group_name="right_arm")): pose = sample_from_boundary(xy_bounds_dict) check_pose = group.get_random_pose() check_pose.pose.position.x = pose[0] check_pose.pose.position.y = pose[1] check_pose.pose.position.z = pose[2] if (not ik_test(check_pose)[0]): joint_angles = ik_test(check_pose)[1] filler_robot_state[10:17] = moveit_scrambler( joint_angles.values()) rs_man.joint_state.position = tuple(filler_robot_state) joint_angles = ik_test(check_pose)[1] masterEnvDict[key_name]['Targets']['Pose'].append(check_pose) masterEnvDict[key_name]['Targets']['Joints'].append( joint_angles) #sceneModifier.delete_obstacles() masterEnvDict[key_name]['ObsPoses'] = new_pose iter_num += 1 with open(envFileName, "wb") as env_f: pickle.dump(masterEnvDict, env_f) print("Done saving... exiting loop \n\n\n") done = True
# # Author: Ioan Sucan import sys import rospy from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown from moveit_msgs.msg import RobotState if __name__=='__main__': roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) robot = RobotCommander() rospy.sleep(1) print "Current state:" print robot.get_current_state() # plan to a random location a = robot.right_arm a.set_start_state(RobotState()) r = a.get_random_joint_values() print "Planning to random joint position: " print r p = a.plan(r) print "Solution:" print p roscpp_shutdown()
class Approach(object): def __init__(self, name, takeoff_height): self.robot_name = name self.takeoff_height = takeoff_height # Mutual exclusion self.sonar_me = Condition() self.odometry_me = Condition() self.current_height = None # Create trajectory server self.trajectory_server = SimpleActionServer( 'approach_server', ExecuteDroneApproachAction, self.goCallback, False) self.server_feedback = ExecuteDroneApproachFeedback() self.server_result = ExecuteDroneApproachResult() # Get client from hector_quadrotor_actions self.move_client = SimpleActionClient("/{}/action/pose".format(name), PoseAction) self.move_client.wait_for_server() # Subscribe to ground_truth to monitor the current pose of the robot rospy.Subscriber("/{}/ground_truth/state".format(name), Odometry, self.poseCallback) # Subscribe to topic to receive the planned trajectory rospy.Subscriber("/{}/move_group/display_planned_path".format(name), DisplayTrajectory, self.planCallback) #Auxiliary variables self.trajectory = [] # Array with the trajectory to be executed self.trajectory_received = False # Flag to signal trajectory received self.odom_received = False # Flag to signal odom received self.robot = RobotCommander( robot_description="{}/robot_description".format(name), ns="/{}".format(name)) self.display_trajectory_publisher = rospy.Publisher( '/{}/move_group/display_planned_path'.format(name), DisplayTrajectory, queue_size=20) # Variables for collision callback self.validity_srv = rospy.ServiceProxy( '/{}/check_state_validity'.format(name), GetStateValidity) self.validity_srv.wait_for_service() self.collision = False # Subscribe to sonar_height rospy.Subscriber("sonar_height", Range, self.sonar_callback, queue_size=10) #Start move_group self.move_group = MoveGroup('earth', name) self.move_group.set_planner(planner_id='RRTConnectkConfigDefault', attempts=10, allowed_time=2) #RRTConnectkConfigDefault self.move_group.set_workspace([XMIN, YMIN, ZMIN, XMAX, YMAX, ZMAX]) # Set the workspace size # Get current robot position to define as start planning point self.current_pose = self.robot.get_current_state() #Start planningScenePublisher self.scene_pub = PlanningScenePublisher(name, self.current_pose) # Start trajectory server self.trajectory_server.start() def sonar_callback(self, msg): ''' Function to update drone height ''' self.sonar_me.acquire() self.current_height = msg.range self.sonar_me.notify() self.sonar_me.release() def poseCallback(self, odometry): ''' Monitor the current position of the robot ''' self.odometry_me.acquire() self.odometry = odometry.pose.pose # print(self.odometry) self.odometry_me.release() self.odom_received = True def goCallback(self, pose): ''' Require a plan to go to the desired target and try to execute it 5 time or return erro ''' self.target = pose.goal #################################################################### # First takeoff if the drone is close to ground self.sonar_me.acquire() while not (self.current_height and self.odometry): self.sonar_me.wait() if self.current_height < self.takeoff_height: self.odometry_me.acquire() takeoff_pose = PoseGoal() takeoff_pose.target_pose.header.frame_id = "{}/world".format( self.robot_name) takeoff_pose.target_pose.pose.position.x = self.odometry.position.x takeoff_pose.target_pose.pose.position.y = self.odometry.position.y takeoff_pose.target_pose.pose.position.z = self.odometry.position.z + self.takeoff_height - self.current_height #Add the heght error to the height takeoff_pose.target_pose.pose.orientation.x = self.odometry.orientation.x takeoff_pose.target_pose.pose.orientation.y = self.odometry.orientation.y takeoff_pose.target_pose.pose.orientation.z = self.odometry.orientation.z takeoff_pose.target_pose.pose.orientation.w = self.odometry.orientation.w self.odometry_me.release() self.move_client.send_goal(takeoff_pose) self.move_client.wait_for_result() result = self.move_client.get_state() if result == GoalStatus.ABORTED: rospy.logerr("Abort approach! Unable to execute takeoff!") self.trajectory_server.set_aborted() return self.sonar_me.release() #################################################################### rospy.loginfo("Try to start from [{},{},{}]".format( self.odometry.position.x, self.odometry.position.y, self.odometry.position.z)) rospy.loginfo("Try to go to [{},{},{}]".format(self.target.position.x, self.target.position.y, self.target.position.z)) self.trials = 0 while self.trials < 5: rospy.logwarn("Attempt {}".format(self.trials + 1)) if (self.trials > 1): self.target.position.z += 2 * rd() - 1 result = self.go(self.target) if (result == 'replan') or (result == 'no_plan'): self.trials += 1 else: self.trials = 10 self.collision = False if result == 'ok': self.trajectory_server.set_succeeded() elif (result == 'preempted'): self.trajectory_server.set_preempted() else: self.trajectory_server.set_aborted() def go(self, target_): ''' Function to plan and execute the trajectory one time ''' # Insert goal position on an array target = [] target.append(target_.position.x) target.append(target_.position.y) target.append(target_.position.z) target.append(target_.orientation.x) target.append(target_.orientation.y) target.append(target_.orientation.z) target.append(target_.orientation.w) #Define target for move_group # self.move_group.set_joint_value_target(target) self.move_group.set_target(target) self.odometry_me.acquire() self.current_pose.multi_dof_joint_state.transforms[ 0].translation.x = self.odometry.position.x self.current_pose.multi_dof_joint_state.transforms[ 0].translation.y = self.odometry.position.y self.current_pose.multi_dof_joint_state.transforms[ 0].translation.z = self.odometry.position.z self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.x self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.y self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.z self.current_pose.multi_dof_joint_state.transforms[ 0].rotation.x = self.odometry.orientation.w self.odometry_me.release() #Set start state self.move_group.set_start_state(self.current_pose) # Plan a trajectory till the desired target plan = self.move_group.plan() if plan.planned_trajectory.multi_dof_joint_trajectory.points: # Execute only if has points on the trajectory while (not self.trajectory_received): rospy.loginfo("Waiting for trajectory!") rospy.sleep(0.2) # rospy.loginfo("TRAJECTORY: {}".format(self.trajectory)) #Execute trajectory with action_pose last_pose = self.trajectory[0] for pose in self.trajectory: # Verify preempt call if self.trajectory_server.is_preempt_requested(): self.move_client.send_goal(last_pose) self.move_client.wait_for_result() self.scene_pub.publishScene() self.trajectory_received = False self.odom_received = False return 'preempted' #Send next pose to move self.next_pose = pose.target_pose.pose self.move_client.send_goal(pose, feedback_cb=self.collisionCallback) self.move_client.wait_for_result() result = self.move_client.get_state() self.scene_pub.publishScene() # Abort if the drone can not reach the position if result == GoalStatus.ABORTED: self.move_client.send_goal( last_pose) #Go back to the last pose self.move_client.wait_for_result() self.trajectory_received = False self.odom_received = False return 'aborted' elif result == GoalStatus.PREEMPTED: # last_pose.target_pose.pose.position.z += rd() - 0.5 self.move_client.send_goal( last_pose) #Go back to the last pose self.move_client.wait_for_result() self.trajectory_received = False self.odom_received = False return 'replan' elif result == GoalStatus.SUCCEEDED: self.trials = 0 last_pose = pose self.server_feedback.current_pose = self.odometry self.trajectory_server.publish_feedback(self.server_feedback) # Reset control variables self.trajectory_received = False self.odom_received = False rospy.loginfo("Trajectory is traversed!") return 'ok' else: rospy.logerr("Trajectory is empty. Planning was unsuccessful.") return 'no_plan' def planCallback(self, msg): ''' Receive planned trajectories and insert it into an array of waypoints ''' if (not self.odom_received): return # Variable to calculate the distance difference between 2 consecutive points last_pose = PoseGoal() last_pose.target_pose.pose.position.x = self.odometry.position.x last_pose.target_pose.pose.position.y = self.odometry.position.y last_pose.target_pose.pose.position.z = self.odometry.position.z last_pose.target_pose.pose.orientation.x = self.odometry.orientation.x last_pose.target_pose.pose.orientation.y = self.odometry.orientation.y last_pose.target_pose.pose.orientation.z = self.odometry.orientation.z last_pose.target_pose.pose.orientation.w = self.odometry.orientation.w self.trajectory = [] last_motion_theta = 0 for t in msg.trajectory: for point in t.multi_dof_joint_trajectory.points: waypoint = PoseGoal() waypoint.target_pose.header.frame_id = "{}/world".format( self.robot_name) waypoint.target_pose.pose.position.x = point.transforms[ 0].translation.x waypoint.target_pose.pose.position.y = point.transforms[ 0].translation.y waypoint.target_pose.pose.position.z = point.transforms[ 0].translation.z # Orientate the robot always to the motion direction delta_x = point.transforms[ 0].translation.x - last_pose.target_pose.pose.position.x delta_y = point.transforms[ 0].translation.y - last_pose.target_pose.pose.position.y motion_theta = atan2(delta_y, delta_x) last_motion_theta = motion_theta # Make the robot orientation fit with the motion orientation if the movemente on xy is bigger than RESOLUTION # if (abs(delta_x) > RESOLUTION) or (abs(delta_y) > RESOLUTION): q = quaternion_from_euler(0, 0, motion_theta) waypoint.target_pose.pose.orientation.x = q[0] waypoint.target_pose.pose.orientation.y = q[1] waypoint.target_pose.pose.orientation.z = q[2] waypoint.target_pose.pose.orientation.w = q[3] # else: # waypoint.target_pose.pose.orientation.x = point.transforms[0].rotation.x # waypoint.target_pose.pose.orientation.y = point.transforms[0].rotation.y # waypoint.target_pose.pose.orientation.z = point.transforms[0].rotation.z # waypoint.target_pose.pose.orientation.w = point.transforms[0].rotation.w # Add a rotation inplace if next position has an angle difference bigger than 45 if abs(motion_theta - last_motion_theta) > 0.785: last_pose.target_pose.pose.orientation = waypoint.target_pose.pose.orientation self.trajectory.append(last_pose) last_pose = copy.copy( waypoint) # Save pose to calc the naxt delta last_motion_theta = motion_theta self.trajectory.append(waypoint) #Insert a last point to ensure that the robot end at the right position waypoint = PoseGoal() waypoint.target_pose.header.frame_id = "{}/world".format( self.robot_name) waypoint.target_pose.pose.position.x = point.transforms[ 0].translation.x waypoint.target_pose.pose.position.y = point.transforms[ 0].translation.y waypoint.target_pose.pose.position.z = point.transforms[ 0].translation.z waypoint.target_pose.pose.orientation.x = point.transforms[ 0].rotation.x waypoint.target_pose.pose.orientation.y = point.transforms[ 0].rotation.y waypoint.target_pose.pose.orientation.z = point.transforms[ 0].rotation.z waypoint.target_pose.pose.orientation.w = point.transforms[ 0].rotation.w self.trajectory.append(waypoint) self.trajectory_received = True def collisionCallback(self, feedback): ''' This callback runs on every feedback message received ''' validity_msg = GetStateValidityRequest( ) # Build message to verify collision validity_msg.group_name = PLANNING_GROUP if self.next_pose and (not self.collision): self.odometry_me.acquire() x = self.odometry.position.x y = self.odometry.position.y z = self.odometry.position.z # Distance between the robot and the next position dist = sqrt((self.next_pose.position.x - x)**2 + (self.next_pose.position.y - y)**2 + (self.next_pose.position.z - z)**2) # Pose to verify collision pose = Transform() pose.rotation.x = self.odometry.orientation.x pose.rotation.y = self.odometry.orientation.y pose.rotation.z = self.odometry.orientation.z pose.rotation.w = self.odometry.orientation.w self.odometry_me.release() #Verify possible collisions on diferent points between the robot and the goal point # rospy.logerr("\n\n\nCOLLISION CALLBACK: ") # rospy.logerr(dist) for d in arange(RESOLUTION, dist + 0.5, RESOLUTION): pose.translation.x = (self.next_pose.position.x - x) * (d / dist) + x pose.translation.y = (self.next_pose.position.y - y) * (d / dist) + y pose.translation.z = (self.next_pose.position.z - z) * (d / dist) + z self.current_pose.multi_dof_joint_state.transforms[ 0] = pose # Insert the correct odometry value validity_msg.robot_state = self.current_pose # Call service to verify collision collision_res = self.validity_srv.call(validity_msg) # print("\nCollision response:") # print(collision_res) # Check if robot is in collision if not collision_res.valid: # print(validity_msg) rospy.logerr('Collision in front [x:{} y:{} z:{}]'.format( pose.translation.x, pose.translation.y, pose.translation.z)) rospy.logerr('Current pose [x:{} y:{} z:{}]'.format( x, y, z)) # print(collision_res) self.move_client.cancel_goal() self.collision = True return
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)
import sys import rospy import moveit_commander import moveit_msgs.msg from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown from moveit_msgs.msg import RobotState if __name__ == '__main__': roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) robot = RobotCommander() rospy.sleep(1) print "Current state:" print robot.get_current_state() # plan to a random location a = robot.arm while not rospy.is_shutdown(): a.set_start_state(RobotState()) r = a.get_random_joint_values() p = a.plan(r) a.execute(p) rospy.sleep(10) # print "Planning to random joint position: " # print r # print "Solution:" # print p
class Baxter(object): def __init__(self): # create a RobotCommander self.robot = RobotCommander() # create a PlanningScene Interface object self.scene = PlanningSceneInterface() # create left arm move group and gripper self.left_arm = Arm("left_arm") self.left_gripper = self.left_arm.gripper = Gripper("left") # create right arm move group and gripper self.right_arm = Arm("right_arm") self.right_gripper = self.right_arm.gripper = Gripper("right") self.gripper_offset = 0.0 # reset robot self.reset() def reset(self, ): """ Reset robot state and calibrate grippers """ # enable robot self.enable() # move to ready position joint_angles = [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0] self.move_to_joints("right_arm", joint_angles, blocking=False) self.move_to_joints("left_arm", joint_angles, blocking=True) # calibrate grippers self.calibrate_grippers() return def enable(self): """ Enable robot """ self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled self._rs.enable() def calibrate_grippers(self): """ Calibrate grippers """ if not self.left_gripper.calibrated(): self.left_gripper.calibrate() if not self.right_gripper.calibrated(): self.right_gripper.calibrate() return def get_robot_state(self): """ Get robot state Returns ros msg (moveit_msgs.msg._RobotState.RobotState) - A message that contains information about all of the joints, including fixed head pan, revolute arm joints, and prismatic finger joints """ return self.robot.get_current_state() def get_group_names(self): """ Get group names Returns group names (list): a list of move groups that can be used for planning """ return self.robot.get_group_names() def pick(self): return def place(self): return
class Arm(): default_planner = "BKPIECEkConfigDefault" link_type_map = { LinkType.INTERMEDIATE: IntermediateLink, LinkType.TERMINAL: TerminalLink } def __init__(self, planner=default_planner, links={"ee": (LinkType.INTERMEDIATE, "manipulator")}): # initialize interface node (if needed) if rospy.get_node_uri() is None: self.node_name = 'ur5_interface_node' rospy.init_node(self.node_name) ROS_INFO( "The Arm interface was created outside a ROS node, a new node will be initialized!" ) else: ROS_INFO( "The Arm interface was created within a ROS node, no need to create a new one!" ) # store parameters self._planner = planner self._links = links # create a RobotCommander self._robot = RobotCommander() # create semaphore self._semaphore = BoundedSemaphore(value=1) # default link (for utility functions only) self._default_link = None # create links objects num_valid_links = 0 for link in self._links.items(): link_name, link_description = link link_type, link_group_name = link_description link_class = Arm.link_type_map[link_type] link_obj = link_class(self, link_group_name, self._planner) if not self._robot.has_group(link_group_name): ROS_ERR("Move group `%s` not found!", link_group_name) else: self.__dict__[link_name] = link_obj self._default_link = link_obj num_valid_links += 1 if num_valid_links <= 0: ROS_ERR("No valid links were found") return None # keep track of the status of the node self._is_shutdown = False self.verbose = False def get_robot_state(self): return self._robot.get_current_state() def get_group_names(self): return self._robot.get_group_names() def get_planning_frame(self): return self._default_link._group.get_planning_frame() def get_joint_values(self): return self._default_link._group.get_current_joint_values() def plan_to_joint_config(self, joint_angles): ''' Plan to a given joint configuration ''' # Clear old pose targets self._default_link._group.clear_pose_targets() # Set Joint configuration target self._default_link._group.set_joint_value_target(joint_angles) numTries = 0 while numTries < 5: success, plan, _, _ = self._default_link._group.plan() numTries += 1 if success: if self.verbose: print('succeeded in %d tries' % numTries) return True, plan if self.verbose: print("Planning failed") return False, None def execute_plan(self, plan): ''' Execute a given plan ''' self._semaphore.acquire() out = self._default_link._group.execute(plan) sleep(0.05) self._semaphore.release() return out def _plan_to_pose(self, group, pose): ''' Plan to a given pose for the end-effector ''' # Clear old pose targets group.clear_pose_targets() constr = group.get_path_constraints() if len(constr.orientation_constraints) > 0: pose.orientation = constr.orientation_constraints[0].orientation # Set target pose group.set_pose_target(pose) numTries = 0 while numTries < 5: success, plan, _, _ = group.plan() numTries += 1 # check that planning succeeded if success: if self.verbose: print('succeeded in %d tries' % numTries) return True, plan # if we get here, we couldn't find a plan in max number of tries if self.verbose: print('Planning failed') return False, None def _plan_cartesian_path(self, group, waypoints): numTries = 0 while numTries < 5: success, plan, _, _ = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0 # jump_threshold ) numTries += 1 # check that planning succeeded if success: if self.verbose: print('succeeded in %d tries' % numTries) return True, plan # if we get here, we couldn't find a plan in max number of tries return False, None def _execute_plan(self, group, plan): ''' Execute a given plan ''' self._semaphore.acquire() out = group.execute(plan) self._semaphore.release() return out def shutdown(self): print('Shutting down Arm...') self._is_shutdown = True print('Arm released!') return True
def main(args): rospy.init_node('path_data_gen') # sometimes takes a few tries to connect to robot arm limb_init = False while not limb_init: try: limb = baxter_interface.Limb('right') limb_init = True except OSError: limb_init = False neutral_start = limb.joint_angles() min_goal_cost_threshold = 2.0 # set speed to make sure execution does not violate the speed constraint limb.set_joint_position_speed(0.65) # Set up planning scene and Move Group objects robot = RobotCommander() group = MoveGroupCommander("right_arm") sv = StateValidity() # Setup tables (geometry and location defined in moveit_functions.py, set_environment function) # set_environment(robot, scene) # Additional Move group setup # group.set_goal_joint_tolerance(0.001) # group.set_max_velocity_scaling_factor(0.7) # group.set_max_acceleration_scaling_factor(0.1) max_time = args.max_time group.set_planning_time(max_time) # Dictionary to save path data, and filename to save the data to in the end pathsDict = {} if not os.path.exists(args.path_data_path): os.makedirs(args.path_data_path) pathsFile = args.path_data_path + args.path_data_file # load data from environment files for obstacle locations and collision free goal poses # with open("env/environment_data/trainEnvironments_GazeboPatch.pkl", "rb") as env_f: with open(args.env_data_path + args.env_data_file, "rb") as env_f: envDict = pickle.load(env_f) # with open("env/environment_data/trainEnvironments_testGoals.pkl", "rb") as goal_f: #with open(args.targets_data_path+args.targets_data_file, "rb") as goal_f: # goalDict = pickle.load(goal_f) # Obstacle data stored in environment dictionary, loaded into scene modifier to apply obstacle scenes to MoveIt robot_state = robot.get_current_state() rs_man = RobotState() rs_man.joint_state.name = robot_state.joint_state.name filler_robot_state = list(robot_state.joint_state.position) goal_sampler = GoalSampler(group, limb, robot_state, sv) # Here we go # slice this if you want only some environments test_envs = envDict['poses'].keys() done = False iter_num = 0 print("Testing envs: ") print(test_envs) joint_state_topic = ['joint_states:=/robot/joint_states'] roscpp_initialize(joint_state_topic) rospy.init_node("path_data_gen") #roscpp_initialize(sys.argv) # parameters for loading point cloud executable = './pcd_to_pointcloud' rootPath = '../data/pcd/' rospy.wait_for_service('clear_octomap') clear_octomap = rospy.ServiceProxy('clear_octomap', Empty) respond = clear_octomap() print(group.get_end_effector_link()) # try moving the robot to current joints #group.go(joints=group.get_current_joint_values()) while (not rospy.is_shutdown() and not done): for i_env, env_name in enumerate(test_envs): print("env iteration number: " + str(i_env)) print("env name: " + str(env_name)) # load point cloud saved name = '' for file in sorted( os.listdir(args.pcd_data_path), key=lambda x: int(x.split('Env_')[1].split('_')[1][:-4])): if (fnmatch.fnmatch(file, env_name + "*")): # if found the first one that matches the name env+something, then record it name = file break call = create_call(executable, rootPath, name) ### Printing the executable call and allowing user to manually cycle through environments for demonstration print(call) ### Uncomment below to call pointcloud_to_pcd executable which takes snapshot of the streaming pointcloud data ### and saves it to a .pcd file in a desired file location (as specified by prefix in the command call) print("Calling executable... \n\n\n") t = time.time() #stderr_f = open('log.txt', 'w') # publishing topic of point cloud for planning # wait for some time to make sure the point cloud is loaded corretly p = subprocess.Popen(call) #, stderr=stderr_f) rospy.sleep(10) new_pose = envDict['poses'][env_name] pathsDict[env_name] = {} pathsDict[env_name]['paths'] = [] pathsDict[env_name]['costs'] = [] pathsDict[env_name]['times'] = [] pathsDict[env_name]['total'] = 0 pathsDict[env_name]['feasible'] = 0 #collision_free_goals = goalDict[env_name]['Joints'] total_paths = 0 feasible_paths = 0 i_path = 0 #group.go(joints=group.get_current_joint_values()) # run until either desired number of total or feasible paths has been found while (total_paths < args.paths_to_save): #do planning and save data # some invalid goal states found their way into the goal dataset, check to ensure goals are "reaching" poses above the table # by only taking goals which have a straight path cost above a threshold valid_goal = False print("FP: " + str(feasible_paths)) print("TP: " + str(total_paths)) total_paths += 1 i_path += 1 # Uncomment below if using a start state different than the robot current state # filler_robot_state = list(robot_state.joint_state.position) #not sure if I need this stuff # filler_robot_state[10:17] = moveit_scrambler(start.values()) # rs_man.joint_state.position = tuple(filler_robot_state) # group.set_start_state(rs_man) # set start pos = [] while not valid_goal: pose, joint = goal_sampler.sample() goal = joint optimal_path = [neutral_start.values(), goal.values()] optimal_cost = compute_cost(optimal_path) if optimal_cost > min_goal_cost_threshold: valid_goal = True group.set_start_state_to_current_state() group.clear_pose_targets() try: group.set_joint_value_target( moveit_scrambler(goal.values())) # set target except MoveItCommanderException as e: print(e) continue start_t = time.time() plan = group.plan() t = time.time() - start_t #group.execute(plan) pos = [ plan.joint_trajectory.points[i].positions for i in range(len(plan.joint_trajectory.points)) ] if pos != []: pos = np.asarray(pos) cost = compute_cost(pos) print("Time: " + str(t)) print("Cost: " + str(cost)) # Uncomment below if using max time as criteria for failure if (t > (max_time * 0.99)): print("Reached max time...") continue feasible_paths += 1 pathsDict[env_name]['paths'].append(pos) pathsDict[env_name]['costs'].append(cost) pathsDict[env_name]['times'].append(t) pathsDict[env_name]['feasible'] = feasible_paths pathsDict[env_name]['total'] = total_paths # Uncomment below if you want to overwrite data on each new feasible path with open(pathsFile + "_" + env_name + ".pkl", "wb") as path_f: pickle.dump(pathsDict[env_name], path_f) print("\n") p.terminate() p.wait() # rosservice call to clear octomap rospy.wait_for_service('clear_octomap') clear_octomap = rospy.ServiceProxy('clear_octomap', Empty) respond = clear_octomap() iter_num += 1 print("Env: " + str(env_name)) print("Feasible Paths: " + str(feasible_paths)) print("Total Paths: " + str(total_paths)) print("\n") pathsDict[env_name]['total'] = total_paths pathsDict[env_name]['feasible'] = feasible_paths with open(pathsFile + "_" + env_name + ".pkl", "wb") as path_f: pickle.dump(pathsDict[env_name], path_f) print("Done iterating, saving all data and exiting...\n\n\n") with open(pathsFile + ".pkl", "wb") as path_f: pickle.dump(pathsDict, path_f) done = True
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) #self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.marker = [] self.position_list = [] self.orientation_list = [] self.m_idd = 0 self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.ar_pose = Pose() self.goalPoseFromAR = Pose() self.br = tf.TransformBroadcaster() #self.goalPose_from_arPose = Pose() self.trans = [] self.rot = [] self.target_ar_id = 9 self.calculed_coke_pose = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): """ move to initial pose of the UR3 """ #self.clear_octomap() planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print "====== current pose : ", self.wpose marker_joint_goal = [ -0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982 ] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) def move_moveit_setting_pose(self, pose_name): """ move to one of the moveit_setup pose: "home", "zeros", "table" """ if pose_name == "home": self.robot_arm.set_named_target("home") elif pose_name == "zeros": self.robot_arm.set_named_target("zeros") elif pose_name == "table": self.robot_arm.set_named_target("table") #print "Press the Enter" #raw_input() self.robot_arm.go(wait=True) def go_to_move(self, scale=1.0): """ manipulator is moving to the target ar_marker. \n the start ar_marker id is 9, goal ar_marker id is 10. """ #self.calculed_coke_pose = self.robot_arm.get_current_pose() planning_frame = self.robot_arm.get_planning_frame() coke_offset = [0, -0.35, -0.1] #x y z # gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품. # linear offset = abs([0, 0.0625, 0.13]) robot_base_offset = 0.873 base_wrist2_offset = 0.1 #for avoiding link contact error if self.target_ar_id == 9: print ">> robot arm plannig frame: \n", planning_frame print ">> move mode id: ", self.target_ar_id self.calculed_coke_pose.position.x = ( scale * self.goal_x) # base_link to wrist2 x-offset self.calculed_coke_pose.position.y = (scale * self.goal_y) + coke_offset[1] #self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset self.calculed_coke_pose.position.z = ( scale * self.goal_z ) + robot_base_offset # world to base_link z-offset and coke can offset self.calculed_coke_pose.orientation = Quaternion( *quaternion_from_euler(3.14, 0, 1.57)) print "========== coke_pose goal frame: ", self.calculed_coke_pose self.robot_arm.set_pose_target(self.calculed_coke_pose) elif self.target_ar_id == 10: print ">> robot arm plannig frame: \n", planning_frame print ">> move mode id: ", self.target_ar_id self.calculed_coke_pose.position.x = (scale * self.goal_x) + coke_offset[1] self.calculed_coke_pose.position.y = (scale * self.goal_y) + 0 self.calculed_coke_pose.position.z = ( scale * self.goal_z ) + robot_base_offset # world to base_link z-offset and coke can offset self.calculed_coke_pose.orientation = Quaternion( *quaternion_from_euler(3.14, 0, 0)) print "========== coke_pose goal frame: ", self.calculed_coke_pose self.robot_arm.set_pose_target(self.calculed_coke_pose) tf_display_position = [ self.calculed_coke_pose.position.x, self.calculed_coke_pose.position.y, self.calculed_coke_pose.position.z ] tf_display_orientation = [ self.calculed_coke_pose.orientation.x, self.calculed_coke_pose.orientation.y, self.calculed_coke_pose.orientation.z, self.calculed_coke_pose.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz coke_waypoints = [] coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose)) (coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path( coke_waypoints, 0.01, 0.0) self.display_trajectory(coke_plan) ## ## ## print "============ Press `Enter` to if plan is correct!! ..." raw_input() self.robot_arm.go(True) def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch, yaw): """ Manipulator is moving to the desired coordinate. \n Now move to the ar_10 marker. """ calculed_ar_id_10 = Pose() #desired_goal_pose = [0.171, -0.113, 1.039] #desired_goal_euler = [3.14, 0.17, 0] desired_goal_pose = [pose_x, pose_y, pose_z] desired_goal_euler = [roll, pitch, yaw] Cplanning_frame = self.robot_arm.get_planning_frame() print ">> current planning frame: \n", Cplanning_frame calculed_ar_id_10.position.x = desired_goal_pose[0] + 0.1 calculed_ar_id_10.position.y = desired_goal_pose[1] calculed_ar_id_10.position.z = desired_goal_pose[2] calculed_ar_id_10.orientation = Quaternion(*quaternion_from_euler( desired_goal_euler[0], desired_goal_euler[1], desired_goal_euler[2])) print ">>> ar id 10 goal frame: ", desired_goal_pose self.robot_arm.set_pose_target(calculed_ar_id_10) tf_display_position = [ calculed_ar_id_10.position.x, calculed_ar_id_10.position.y, calculed_ar_id_10.position.z ] tf_display_orientation = [ calculed_ar_id_10.orientation.x, calculed_ar_id_10.orientation.y, calculed_ar_id_10.orientation.z, calculed_ar_id_10.orientation.w ] jj = 0 while jj < 5: jj += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz ar_id_10_waypoints = [] ar_id_10_waypoints.append(copy.deepcopy(calculed_ar_id_10)) (ar_id_10_plan, ar_id_10_fraction) = self.robot_arm.compute_cartesian_path( ar_id_10_waypoints, 0.01, 0.0) self.display_trajectory(ar_id_10_plan) ## ## ## print "============ Press `Enter` to if plan is correct!! ..." raw_input() self.robot_arm.go(True) def display_trajectory(self, plan): """ Visualized trajectory of the manipulator """ display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0): """ Working on the "linear move": x-axis -> y-axis -> z-axis """ waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10 #print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # waypoints.append(copy.deepcopy(self.wpose)) ii += 1 #print "waypoints:", waypoints (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_x(self, x_offset, scale=1.0): """ Working on the "linear move": x-axis """ waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_y(self, y_offset, scale=1.0): """ Working on the "linear move": y-axis """ waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset #-0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_z(self, z_offset, scale=1.0): """ Working on the "linear move": z-axis """ waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.z = (scale * self.wpose.position.z) + z_offset #-0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): """ execute planned "plan" trajectory. """ group = self.robot_arm group.execute(plan, wait=True) def ar_pose_subscriber(self): """ ar_maker subscriber """ rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") #print "======= pos(meter): ", self.position_list #print "======= orientation: ", self.orientation_list def ar_tf_listener(self, msg): """ Listening the ar_marker pose coordinate. """ try: self.marker = msg.markers ml = len(self.marker) target_start_point_id = self.target_ar_id #target_id = target_ar_id #self.m_idd = self.marker[0].id # 임시용 for ii in range(0, ml): # 0 <= ii < ml self.m_idd = self.marker[ii].id #print "checked all id: ", self.m_idd if self.m_idd != target_start_point_id: pass #target_id_flage = False elif self.m_idd == target_start_point_id: target_id_flage = True target_id = self.m_idd target_id_index = ii #print "target id: ", target_id_index, target_id, target_id_flage if target_id_flage == True: self.ar_pose.position.x = self.marker[ target_id_index].pose.pose.position.x self.ar_pose.position.y = self.marker[ target_id_index].pose.pose.position.y self.ar_pose.position.z = self.marker[ target_id_index].pose.pose.position.z self.ar_pose.orientation.x = self.marker[ target_id_index].pose.pose.orientation.x self.ar_pose.orientation.y = self.marker[ target_id_index].pose.pose.orientation.y self.ar_pose.orientation.z = self.marker[ target_id_index].pose.pose.orientation.z self.ar_pose.orientation.w = self.marker[ target_id_index].pose.pose.orientation.w self.goal_x = self.ar_pose.position.x self.goal_y = self.ar_pose.position.y self.goal_z = self.ar_pose.position.z self.position_list = [self.goal_x, self.goal_y, self.goal_z] self.orientation_list = [ self.ar_pose.orientation.x, self.ar_pose.orientation.y, self.ar_pose.orientation.z, self.ar_pose.orientation.w ] (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion( self.orientation_list) #list form으로 넘겨주어야 함 #print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z #print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw #print "ar_pos(meter): \n", self.position_list #print "ar_orientation: \n", self.orientation_list except: return
try: rospy.init_node("ik_node") robot = RobotCommander() group_name = "manipulator" group = MoveGroupCommander(group_name) connection = rospy.ServiceProxy("/compute_ik", GetPositionIK) rospy.loginfo("waiting for ik server") connection.wait_for_service() rospy.loginfo("server found") request = GetPositionIKRequest() request.ik_request.group_name = group_name request.ik_request.ik_link_name = group.get_end_effector_link() request.ik_request.ik_link_names = robot.get_link_names(group_name) request.ik_request.robot_state = robot.get_current_state() # get cartesian state cartesian = raw_input("enter cartesian state x y z r p y \n") [x, y, z, roll, pitch, yaw] = [float(idx) for idx in cartesian.split(' ')] request.ik_request.pose_stamped.pose.position.x = x request.ik_request.pose_stamped.pose.position.y = y request.ik_request.pose_stamped.pose.position.z = z quat = quaternion_from_euler(roll, pitch, yaw) request.ik_request.pose_stamped.pose.orientation.x = quat[0] request.ik_request.pose_stamped.pose.orientation.y = quat[1] request.ik_request.pose_stamped.pose.orientation.z = quat[2] request.ik_request.pose_stamped.pose.orientation.w = quat[3]
def main(): rospy.init_node(sys.argv[1]) # sometimes takes a few tries to connect to robot arm limb_init = False while not limb_init: try: limb = baxter_interface.Limb('right') limb_init = True except OSError: limb_init = False neutral_start = limb.joint_angles() min_goal_cost_threshold = 2.0 # Set up planning scene and Move Group objects scene = PlanningSceneInterface() scene._scene_pub = rospy.Publisher( 'planning_scene', PlanningScene, queue_size=0) robot = RobotCommander() group = MoveGroupCommander("right_arm") sv = StateValidity() set_environment(robot, scene) # Setup tables (geometry and location defined in moveit_functions.py, set_environment function) # set_environment(robot, scene) # Additional Move group setup # group.set_goal_joint_tolerance(0.001) # group.set_max_velocity_scaling_factor(0.7) # group.set_max_acceleration_scaling_factor(0.1) max_time = 300 group.set_planning_time(max_time) # Dictionary to save path data, and filename to save the data to in the end pathsDict = {} pathsFile = "data/path_data_example" # load data from environment files for obstacle locations and collision free goal poses with open("env/trainEnvironments.pkl", "rb") as env_f: envDict = pickle.load(env_f) with open("env/trainEnvironments_testGoals.pkl", "rb") as goal_f: goalDict = pickle.load(goal_f) # Obstacle data stored in environment dictionary, loaded into scene modifier to apply obstacle scenes to MoveIt sceneModifier = PlanningSceneModifier(envDict['obsData']) sceneModifier.setup_scene(scene, robot, group) robot_state = robot.get_current_state() rs_man = RobotState() rs_man.joint_state.name = robot_state.joint_state.name # Here we go test_envs = envDict['poses'].keys() #slice this if you want only some environments done = False iter_num = 0 print("Testing envs: ") print(test_envs) while(not rospy.is_shutdown() and not done): for i_env, env_name in enumerate(test_envs): print("env iteration number: " + str(i_env)) print("env name: " + str(env_name)) sceneModifier.delete_obstacles() new_pose = envDict['poses'][env_name] sceneModifier.permute_obstacles(new_pose) print("Loaded new pose and permuted obstacles\n") pathsDict[env_name] = {} pathsDict[env_name]['paths'] = [] pathsDict[env_name]['costs'] = [] pathsDict[env_name]['times'] = [] pathsDict[env_name]['total'] = 0 pathsDict[env_name]['feasible'] = 0 collision_free_goals = goalDict[env_name]['Joints'] total_paths = 0 feasible_paths = 0 i_path = 0 while (total_paths < 30): #run until either desired number of total or feasible paths has been found #do planning and save data # some invalid goal states found their way into the goal dataset, check to ensure goals are "reaching" poses above the table # by only taking goals which have a straight path cost above a threshold valid_goal = False while not valid_goal: goal = collision_free_goals[np.random.randint(0, len(collision_free_goals))] optimal_path = [neutral_start.values(), goal.values()] optimal_cost = compute_cost(optimal_path) if optimal_cost > min_goal_cost_threshold: valid_goal = True print("FP: " + str(feasible_paths)) print("TP: " + str(total_paths)) total_paths += 1 i_path += 1 # Uncomment below if using a start state different than the robot current state # filler_robot_state = list(robot_state.joint_state.position) #not sure if I need this stuff # filler_robot_state[10:17] = moveit_scrambler(start.values()) # rs_man.joint_state.position = tuple(filler_robot_state) # group.set_start_state(rs_man) # set start group.set_start_state_to_current_state() group.clear_pose_targets() try: group.set_joint_value_target(moveit_scrambler(goal.values())) # set target except MoveItCommanderException as e: print(e) continue start_t = time.time() plan = group.plan() pos = [plan.joint_trajectory.points[i].positions for i in range(len(plan.joint_trajectory.points))] if pos != []: pos = np.asarray(pos) cost = compute_cost(pos) t = time.time() - start_t print("Time: " + str(t)) print("Cost: " + str(cost)) # Uncomment below if using max time as criteria for failure if (t > (max_time*0.99)): print("Reached max time...") continue feasible_paths += 1 pathsDict[env_name]['paths'].append(pos) pathsDict[env_name]['costs'].append(cost) pathsDict[env_name]['times'].append(t) pathsDict[env_name]['feasible'] = feasible_paths pathsDict[env_name]['total'] = total_paths # Uncomment below if you want to overwrite data on each new feasible path with open (pathsFile + "_" + env_name + ".pkl", "wb") as path_f: pickle.dump(pathsDict[env_name], path_f) print("\n") sceneModifier.delete_obstacles() iter_num += 1 print("Env: " + str(env_name)) print("Feasible Paths: " + str(feasible_paths)) print("Total Paths: " + str(total_paths)) print("\n") pathsDict[env_name]['total'] = total_paths pathsDict[env_name]['feasible'] = feasible_paths with open(pathsFile + "_" + env_name + ".pkl", "wb") as path_f: pickle.dump(pathsDict[env_name], path_f) print("Done iterating, saving all data and exiting...\n\n\n") with open(pathsFile + ".pkl", "wb") as path_f: pickle.dump(pathsDict, path_f) done = True
class trajectoryConstructor(): def __init__(self): rospy.loginfo("Waiting for service " + IK_SERVICE_NAME) rospy.wait_for_service(IK_SERVICE_NAME) self.ik_serv = rospy.ServiceProxy(IK_SERVICE_NAME, GetPositionIK) self.joint_list = ['torso_1_joint', 'torso_2_joint', 'head_1_joint', 'head_2_joint', 'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint', 'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint'] self.left_arm = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint'] self.right_arm = ['arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint'] self.right_arm_torso = ['torso_1_joint', 'torso_2_joint', 'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint'] self.left_arm_torso = ['torso_1_joint', 'torso_2_joint', 'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint'] # # Store answer so we ask only one time # self.joint_state = JointState() # self.joint_state.name = gksi_answer.kinematic_solver_info.joint_names # self.joint_state.position = [0.0] * len(gksi_answer.kinematic_solver_info.joint_names) # self.ik_link_name = gksi_answer.kinematic_solver_info.link_names[0] self.r_commander = RobotCommander() self.initial_robot_state = self.r_commander.get_current_state() if DEBUG_MODE: self.pub_ok_markers = rospy.Publisher('ik_ok_marker_list', MarkerArray, latch=True) self.ok_markers = MarkerArray() self.pub_fail_markers = rospy.Publisher('ik_fail_marker_list', MarkerArray, latch=True) self.fail_markers = MarkerArray() self.markers_id = 5 def getIkPose(self, pose, group="right_arm", previous_state=None): """Get IK of the pose specified, for the group specified, optionally using the robot_state of previous_state (if not, current robot state will be requested) """ # point point to test if there is ik # returns the answer of the service rqst = GetPositionIKRequest() rqst.ik_request.avoid_collisions = True rqst.ik_request.group_name = group rqst.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now()) rqst.ik_request.pose_stamped.header.frame_id = 'base_link' # Set point to check IK for rqst.ik_request.pose_stamped.pose.position = pose.position rqst.ik_request.pose_stamped.pose.orientation = pose.orientation if previous_state == None: cs = self.r_commander.get_current_state() rqst.ik_request.robot_state = cs else: rqst.ik_request.robot_state = previous_state ik_answer = GetPositionIKResponse() if DEBUG_MODE: timeStart = rospy.Time.now() ik_answer = self.ik_serv.call(rqst) if DEBUG_MODE: durationCall= rospy.Time.now() - timeStart rospy.loginfo("Call took: " + str(durationCall.to_sec()) + "s") return ik_answer def computeJointTrajFromCartesian(self, points, arm="right_arm_torso"): #fjt_goal = FollowJointTrajectoryGoal() poselist = [] for point in points: qt = quaternion_from_euler(point.positions[3], point.positions[4], point.positions[5]) pose = Pose(Point(point.positions[0], point.positions[1], point.positions[2]), Quaternion(*qt.tolist())) poselist.append(pose) fjt_goal = self.computeIKsPose(poselist, arm) return fjt_goal def computeIKsPose(self, poselist, arm="right_arm"): rospy.loginfo("Computing " + str(len(poselist)) + " IKs" ) fjt_goal = FollowJointTrajectoryGoal() if arm == 'right_arm_torso': fjt_goal.trajectory.joint_names = self.right_arm_torso elif arm == 'left_arm_torso': fjt_goal.trajectory.joint_names = self.left_arm_torso elif arm == 'right_arm': fjt_goal.trajectory.joint_names = self.right_arm elif arm == 'left_arm': fjt_goal.trajectory.joint_names = self.left_arm # TODO: add other options ik_answer = None for pose in poselist: if ik_answer != None: ik_answer = self.getIkPose(pose,"right_arm", previous_state=ik_answer.solution) else: ik_answer = self.getIkPose(pose) if DEBUG_MODE: rospy.loginfo("Got error_code: " + str(ik_answer.error_code.val) + " which means: " + moveit_error_dict[ik_answer.error_code.val]) if moveit_error_dict[ik_answer.error_code.val] == 'SUCCESS': if DEBUG_MODE: arrow = self.createArrowMarker(pose, ColorRGBA(0,1,0,1)) self.ok_markers.markers.append(arrow) jtp = JointTrajectoryPoint() #ik_answer = GetConstraintAwarePositionIKResponse() # sort positions and add only the ones of the joints we are interested in positions = self.sortOutJointList(fjt_goal.trajectory.joint_names, ik_answer.solution.joint_state) jtp.positions = positions # TODO: add velocities | WILL BE DONE OUTSIDE # TODO: add acc? | DUNNO fjt_goal.trajectory.points.append(jtp) if DEBUG_MODE: self.pub_ok_markers.publish(self.ok_markers) else: if DEBUG_MODE: arrow = self.createArrowMarker(pose, ColorRGBA(1,0,0,1)) self.fail_markers.markers.append(arrow) self.pub_fail_markers.publish(self.fail_markers) return fjt_goal def sortOutJointList(self, joint_name_list, joint_state): """ Get only the joints we are interested in and it's values and return it in joint_state.name and joint_state.points format""" if DEBUG_MODE: rospy.loginfo("Sorting jointlist...") list_to_iterate = joint_name_list curr_j_s = joint_state ids_list = [] position_list = [] for joint in list_to_iterate: idx_in_message = curr_j_s.name.index(joint) ids_list.append(idx_in_message) position_list.append(curr_j_s.position[idx_in_message]) return position_list def adaptTimesAndVelocitiesOfMsg(self, trajectory, plan, desired_final_time): """Adapt the times and velocities of the message for the controller from the times computed in the DMP and velocities 0.0, controller will take care of it""" rospy.loginfo("Adapting times and velocities...") traj = trajectory #FollowJointTrajectoryGoal() p = plan #GetDMPPlanResponse() # we should have the same number of points on each, NOPE, as we can have points that IK fails # if len(traj.trajectory.points) != len(p.plan.points): # rospy.logerr("Oops, something went wrong, different number of points") # rospy.logerr("generated trajectory: " + str(len(traj.trajectory.points)) + " plan: " + str(len(p.plan.points))) # exit(0) point = JointTrajectoryPoint() counter = 0 for point in traj.trajectory.points: #rospy.loginfo("Step " + str(counter) + " / " + str(len(traj.trajectory.points))) counter += 1 point.velocities.extend(len(point.positions) * [0.0]) # adding 0.0 as speeds point.time_from_start = rospy.Duration( counter * (desired_final_time / len(traj.trajectory.points)) ) return traj def publish_markers(self): while True: self.pub_ok_markers.publish(self.ok_markers) self.pub_fail_markers.publish(self.fail_markers) rospy.sleep(0.1) def createArrowMarker(self, pose, color): marker = Marker() marker.header.frame_id = 'base_link' marker.type = marker.ARROW marker.action = marker.ADD general_scale = 0.01 marker.scale.x = general_scale marker.scale.y = general_scale / 3.0 marker.scale.z = general_scale / 10.0 marker.color = color marker.pose.orientation = pose.orientation marker.pose.position = pose.position marker.id = self.markers_id self.markers_id += 1 return marker
def main(argv): rospy.init_node("PickPlaceDemo") rospy.on_shutdown(onshutdownhook) rospy.wait_for_service("descartes_generate_motion_plan") print "Planning service availabe" global scene scene = PlanningSceneInterface() robot = RobotCommander() path = JointTrajectory() rospy.sleep(2) discretization = math.pi / 30.0 #scene.is_diff = True quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0) p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.700 p.pose.position.y = 0.000 p.pose.position.z = 0.060 p.pose.orientation.x = quaternion[0] p.pose.orientation.y = quaternion[1] p.pose.orientation.z = quaternion[2] p.pose.orientation.w = quaternion[3] scene.add_box("box1", p, (0.15, 0.15, 0.15)) p.pose.position.x = 0.700 p.pose.position.y = 0.250 p.pose.position.z = 0.060 scene.add_box("box2", p, (0.15, 0.15, 0.15)) p.pose.position.x = 0.700 p.pose.position.y = 0.500 p.pose.position.z = 0.060 scene.add_box("box3", p, (0.15, 0.15, 0.15)) rospy.sleep(2) del quaternion #Pick the first box quaternion = tf.transformations.quaternion_from_euler( 0.0, (math.pi / 2.0), (math.pi / 2.0)) poses = PoseArray() pose_target = Pose() pose_target.position.x = 0.700 pose_target.position.y = 0.000 pose_target.position.z = 0.155 pose_target.orientation.x = quaternion[0] pose_target.orientation.y = quaternion[1] pose_target.orientation.z = quaternion[2] pose_target.orientation.w = quaternion[3] poses.poses.append(pose_target) try: planningrequest = rospy.ServiceProxy('descartes_generate_motion_plan', generate_motion_plan) plan = planningrequest(poses, "world", "ee_link", rospy.get_param("/controller_joint_names"), 5, "Y_AXIS", discretization, robot.get_current_state().joint_state) path = plan.plan print "Plan generated" except rospy.ServiceException, e: print "Service call failed: %s" % e
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.listener = tf.TransformListener() display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20) self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.position_list = [] self.orientation_list = [] self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.5) self.robot_arm.set_planning_time(10) self.robot_arm.set_num_planning_attempts(10) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print"====== current pose(1) : ", self.wpose marker_joint_goal = [4.721744537353516, -0.7451499144183558, -1.6199515501605433, -1.2175200621234339, 1.6366002559661865, 3.1263363361358643] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) self.wpose = self.robot_arm.get_current_pose() print"====== current pose(2) : ", self.wpose def execute_plan(self,plan): group = self.robot_arm group.execute(plan, wait=True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory)
class Phantomx_Pincher(): def __init__(self): roscpp_initialize([]) rospy.sleep(5) #wait for moveit. TO-DO self.robot = RobotCommander() self.init_planner() def init_planner(self): # initialize the planner parameters self.robot.pincher_arm.set_start_state(RobotState()) self.robot.pincher_arm.set_num_planning_attempts(10000) self.robot.pincher_arm.set_planning_time(5) self.robot.pincher_arm.set_goal_position_tolerance(0.01) self.robot.pincher_arm.set_goal_orientation_tolerance(0.5) def set_start_state_to_current_state(self): self.robot.pincher_arm.set_start_state_to_current_state() self.robot.pincher_gripper.set_start_state_to_current_state() def openGripper(self): #open gripper posture = dict() posture["PhantomXPincher_gripperClose_joint"] = 0.030 self.robot.pincher_gripper.set_joint_value_target(posture) gplan = self.robot.pincher_gripper.plan() if len(gplan.joint_trajectory.points) == 0: return None return gplan def closeGripper(self): # close gripper posture = dict() posture["PhantomXPincher_gripperClose_joint"] = 0.015 self.robot.pincher_gripper.set_joint_value_target(posture) gplan = self.robot.pincher_gripper.plan() if len(gplan.joint_trajectory.points) == 0: return None return gplan def ef_pose(self, target, attemps=10): # Here we try to verify if the target is in the arm range. Also, we # try to orient the end-effector(ef) to nice hard-coded orientation # Returns: planned trajectory self.robot.get_current_state() d = pow(pow(target[0], 2) + pow(target[1], 2), 0.5) if d > 3.0: rospy.loginfo("Too far. Out of reach") return None rp = np.pi / 2.0 - np.arcsin((d - 0.1) / .205) ry = np.arctan2(target[1], target[0]) attemp = 0 again = True while (again and attemp < 10): again = False rp_a = attemp * ((0.05 + 0.05) * np.random.ranf() - 0.05) + rp ry_a = attemp * ((0.05 + 0.05) * np.random.ranf() - 0.05) + ry q = tf.transformations.quaternion_from_euler(0, rp_a, ry_a) print[0, rp_a, ry_a] p = geometry_msgs.msg.Pose() p.position.x = target[0] p.position.y = target[1] p.position.z = target[2] p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] target[2] += np.abs(np.cos(rp)) / 250.0 self.robot.pincher_arm.set_pose_target(p) planed = self.robot.pincher_arm.plan() if len(planed.joint_trajectory.points) == 0: rospy.loginfo("Motion plan failed, trying again") attemp += 1 again = True if again: return None return planed def arm_execute(self, plan): self.robot.pincher_arm.execute(plan) def gripper_execute(self, plan): self.robot.pincher_gripper.execute(plan)
class Phantomx_Pincher(): def __init__(self): roscpp_initialize([]) rospy.sleep(8) # TO-DO: wait for moveit. self.listener = tf.TransformListener() self.robot = RobotCommander() self.init_planner() self.gripper_dimensions = [0.02, 0.02, 0.02] def init_planner(self): # initialize the planner parameters self.robot.pincher_arm.set_start_state(RobotState()) self.robot.pincher_arm.set_num_planning_attempts(10000) self.robot.pincher_arm.set_planning_time(7) self.robot.pincher_arm.set_goal_position_tolerance(0.01) self.robot.pincher_arm.set_goal_orientation_tolerance(0.1) def set_start_state_to_current_state(self): self.robot.pincher_arm.set_start_state_to_current_state() self.robot.pincher_gripper.set_start_state_to_current_state() def openGripper(self): # open gripper posture = dict() posture["PhantomXPincher_gripperClose_joint"] = 0.030 self.robot.pincher_gripper.set_joint_value_target(posture) gplan = self.robot.pincher_gripper.plan() if len(gplan.joint_trajectory.points) == 0: return None return gplan def closeGripper(self): # close gripper posture = dict() posture["PhantomXPincher_gripperClose_joint"] = 0.010 self.robot.pincher_gripper.set_joint_value_target(posture) gplan = self.robot.pincher_gripper.plan() if len(gplan.joint_trajectory.points) == 0: return None return gplan def to_pose(self, waypoint, frame="/map"): p = geometry_msgs.msg.PoseStamped() p.header.frame_id = frame p.header.stamp = rospy.Time.now() p.pose.position.x = waypoint[0] p.pose.position.y = waypoint[1] p.pose.position.z = waypoint[2] p.pose.orientation.w = 1 if len(waypoint) == 7: p.pose.orientation.x = waypoint[3] p.pose.orientation.y = waypoint[4] p.pose.orientation.z = waypoint[5] p.pose.orientation.w = waypoint[6] elif len(waypoint) == 6: q = tf.transformations.quaternion_from_euler( waypoint[3] / 180.0 * np.pi, waypoint[4] / 180.0 * np.pi, waypoint[5] / 180.0 * np.pi) p.pose.orientation.x = q[0] p.pose.orientation.y = q[1] p.pose.orientation.z = q[2] p.pose.orientation.w = q[3] return p def target_to_frame(self, target, frame_to="/arm_base_link", frame_from="/map", orientation=False): pose = self.to_pose(target, frame=frame_from) self.listener.waitForTransform(frame_from, frame_to, rospy.Time.now(), rospy.Duration(4)) self.listener.getLatestCommonTime(frame_from, frame_to) pose = self.listener.transformPose(frame_to, pose) if orientation: return [ pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w ] return [ pose.pose.position.x, pose.pose.position.y, pose.pose.position.z ] def correct_grasp_position(self, obj_dimension, gripper_orientation): ''' This function corrects the grasp position accordantly to the object dimensions, the gripper size (available size for the gripper grasp) and the gripper orientation. Input: - obj_dimension: list with object dimension in meters along x,y,z - gripper_orientation: [roll, pitch, yaw] in radians ''' if len(obj_dimension) < 3: return np.asarray([0, 0, 0]) obj_dimension = np.asarray(obj_dimension) # rospy.loginfo("Obj Dimension [%s]", obj_dimension) delta = (obj_dimension - self.gripper_dimensions) / 2. delta = list(delta) delta[2] = 0 delta[1] = 0.01 # gripper_link # Tranformation Matrix from gripper to arm_base T_G_B = tf.transformations.euler_matrix(gripper_orientation[0], gripper_orientation[1], gripper_orientation[2]) gripper_delta = T_G_B * np.asmatrix(delta + [1]).T gripper_delta = gripper_delta[:3] rospy.loginfo("Delta [arm_base_link] [%s]", gripper_delta) return np.asarray(gripper_delta).flatten() def ef_pose(self, target, dimension=[], orientation=[], attemps=10, frame='/map'): ''' Here we try to verify if the target is in the arm range. Also, we try to orient the end-effector(ef) to nice hard-coded orientation Returns: planned trajectory ''' self.robot.get_current_state() target = self.target_to_frame(target, frame_from=frame) # rospy.loginfo("Planing to [%s] on arm_frame", target) d = pow(pow(target[0], 2) + pow(target[1], 2), 0.5) if d > 0.3: rospy.loginfo("Too far. Out of reach") return None if len(orientation) > 0: rpy = tf.transformations.euler_from_quaternion(orientation) # rospy.loginfo('Robot target quaternion [%s]', # np.rad2deg(orientation)) rp = rpy[1] else: rp = np.pi / 2.0 - np.arcsin((d - 0.1) / .205) ry = np.arctan2(target[1], target[0]) rospy.loginfo("Original target %s %s", target, np.rad2deg([0, rp, ry])) delta = self.correct_grasp_position(dimension, [0, rp, ry]) target -= delta rospy.loginfo(target) ry = np.arctan2(target[1], target[0]) rospy.loginfo(ry) rospy.loginfo("Target after correction %s %s", target, np.rad2deg([0, rp, ry])) attemp = 0 again = True while (again and attemp < 10): again = False rp_a = attemp * ((0.05 + 0.05) * np.random.ranf() - 0.05) + rp ry_a = ry # attemp * ((0.05 + 0.05)*np.random.ranf() - 0.05) + ry q = tf.transformations.quaternion_from_euler(0, rp_a, ry_a) rospy.loginfo([0, rp_a, ry_a]) p = geometry_msgs.msg.PoseStamped() p.header.frame_id = '/arm_base_link' p.pose.position.x = target[0] p.pose.position.y = target[1] p.pose.position.z = target[2] # + np.abs(np.cos(rp))/50.0 p.pose.orientation.x = q[0] p.pose.orientation.y = q[1] p.pose.orientation.z = q[2] p.pose.orientation.w = q[3] rospy.loginfo( "New target pose [%.4f, %.4f, %.4f] [d: %.4f, p: %.4f, y: %.4f]", p.pose.position.x, p.pose.position.y, p.pose.position.z, d, rp_a, ry_a) self.robot.pincher_arm.set_pose_target(p) planed = self.robot.pincher_arm.plan() if len(planed.joint_trajectory.points) == 0: rospy.loginfo("Motion plan failed, trying again") attemp += 1 again = True if again: return None return planed def arm_execute(self, plan): return self.robot.pincher_arm.execute(plan) def gripper_execute(self, plan): return self.robot.pincher_gripper.execute(plan)
# # Author: Ioan Sucan import sys import rospy from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown from moveit_msgs.msg import RobotState if __name__ == "__main__": roscpp_initialize(sys.argv) rospy.init_node("moveit_py_demo", anonymous=True) robot = RobotCommander() rospy.sleep(1) print("Current state:") print(robot.get_current_state()) # plan to a random location a = robot.right_arm a.set_start_state(RobotState()) r = a.get_random_joint_values() print("Planning to random joint position: ") print(r) p = a.plan(r) print("Solution:") print(p) roscpp_shutdown()
# ------------------------------ # Configure the planner # ----------------------------- robot = RobotCommander() robot.pincher_arm.set_start_state(RobotState()) #robot.pincher_arm.set_planner_id('RRTConnectkConfigDefault') robot.pincher_arm.set_num_planning_attempts(10000) robot.pincher_arm.set_planning_time(5) robot.pincher_arm.set_goal_position_tolerance(0.01) robot.pincher_arm.set_goal_orientation_tolerance(0.1) robot.pincher_gripper.set_goal_position_tolerance(0.01) robot.pincher_gripper.set_goal_orientation_tolerance(0.1) robot.get_current_state() robot.pincher_arm.set_start_state(RobotState()) '''#------------------------------ # The Grasp #----------------------------- the_grasp = Grasp() the_grasp.id = "Por cima" # Gripper Posture before the grasp (opened griper) the_grasp.pre_grasp_posture.joint_names = robot.pincher_gripper.get_active_joints() the_grasp.pre_grasp_posture.points.append(JointTrajectoryPoint()) the_grasp.pre_grasp_posture.points[0].positions = [0.030] # Gripper posture while grasping (closed gripper)
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)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback) rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.ii = 0 global goal_x global goal_y global goal_z # self.listener = tf.TransformListener() #self.goal_x = 0 #self.goal_y = 0 #self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 # self.marker = [] self.tomato = [] self.position_list = [] self.orientation_list = [] self.t_idd = 0 self.t_pose_x = [] self.t_pose_y = [] self.t_pose_z = [] self.t_ori_w = [] self.t_ori_x = [] self.t_ori_y = [] self.t_ori_z = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print("====== current pose : ", self.wpose) joint_goal = [ -0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982 ] print("INIT POSE: ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait=True) def go_to_move(self, scale=1.0): planning_frame = self.robot_arm.get_planning_frame() tomato_offset = [0, -0.35, -0.1] robot_base_offset = 0.873 base_wrist2_offset = 0.1 print(">> robot arm planning frame: \n", planning_frame) ''' self.calculated_tomato.position.x = (scale*self.goal_x) self.calculated_tomato.position.y = (scale*self.goal_y) self.calculated_tomato.position.z = (scale*self.goal_z) ''' self.calculated_tomato.position.x = (scale * goal_x) self.calculated_tomato.position.y = (scale * goal_y) self.calculated_tomato.position.z = (scale * goal_z) self.calculated_tomato.orientation = Quaternion( *quaternion_from_euler(3.14, 0.1, 1.57)) print("=== robot_pose goal frame: ", self.calculated_tomato) self.robot_arm.set_pose_target(self.calculated_tomato) tf_display_position = [ self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z ] tf_display_orientation = [ self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato)) (tomato_plan, tomato_offset) = self.robot_arm.compute_cartesian_path( tomato_waypoints, 0.01, 0.0) self.display_trajectory(tomato_plan) print("=== Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch, yaw): ''' Manipulator is moving to the desired coordinate Now move to the calculated_tomato_id_goal ''' calculated_tomato_id_goal = Pose() desired_goal_pose = [pose_x, pose_y, pose_z] desired_goal_euler = [roll, pitch, yaw] Cplanning_frame = self.robot_arm.get_planning_frame() print(">> current planning frame: \n", Cplanning_frame) calculated_tomato_id_goal.position.x = desired_goal_pose[0] calculated_tomato_id_goal.position.y = desired_goal_pose[1] calculated_tomato_id_goal.position.z = desired_goal_pose[2] calculated_tomato_id_goal.orientation = Quaternion( *quaternion_from_euler(desired_goal_euler[0], desired_goal_euler[1], desired_goal_euler[2])) print(">> tomato_id_goal frame: ", desired_goal_pose) self.robot_arm.set_pose_target(calculated_tomato_id_goal) tf_display_position = [ calculated_tomato_id_goal.position.x, calculated_tomato_id_goal.position.x, calculated_tomato_id_goal.position.z ] tf_display_orientation = [ calculated_tomato_id_goal.orientation.x, calculated_tomato_id_goal.orientation.y, calculated_tomato_id_goal.orientation.z, calculated_tomato_id_goal.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz tomato_id_goal_waypoints = [] tomato_id_goal_waypoints.append( copy.deepcopy(calculated_tomato_id_goal)) (tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path( tomato_id_goal_waypoints, 0.01, 0.0) self.display_trajectory(tomato_id_goal_plan) print("============ Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_x(self, x_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_y(self, y_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_z(self, z_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback) rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.detected_names = rospy.get_param( '/darknet_ros/yolo_model/detection_classes/names') self.object_pose_sub = rospy.Subscriber( '/cluster_decomposer/centroid_pose_array', PoseArray, self.collectJsk) self.listener = tf.TransformListener() display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.ii = 0 global goal_x global goal_y global goal_z global ori_w global ori_x global ori_y global ori_z self.distance = 0 self.tomato = [] self.position_list = [] self.orientation_list = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.calculated_tomato_coor = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(10) self.robot_arm.set_num_planning_attempts(10) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print "====== current pose : ", self.wpose marker_joint_goal = [ 2.6482303142547607, -0.3752959410296839, -2.1118043104754847, -0.4801228682147425, -1.4944542090045374, -4.647655431424276 ] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray, self.collectJsk) def go_to_goal_point(self, scale=1.0): planning_frame = self.robot_arm.get_planning_frame() print(">> robot arm planning frame: \n", planning_frame) self.calculated_tomato.position.x = (scale * goal_x) self.calculated_tomato.position.y = (scale * goal_y) self.calculated_tomato.position.z = (scale * goal_z) self.calculated_tomato.orientation.w = (scale * ori_w) self.calculated_tomato.orientation.x = (scale * ori_x) self.calculated_tomato.orientation.y = (scale * ori_y) self.calculated_tomato.orientation.z = (scale * ori_z) print(">> robot_pose goal frame: ", self.calculated_tomato) self.robot_arm.set_pose_target(self.calculated_tomato) tf_display_position = [ self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z ] tf_display_orientation = [ self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato)) (tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path( tomato_waypoints, 0.01, 0.0) self.display_trajectory(tomato_plan) print("======= Press `Enter` to if plan in correct!======") raw_input() self.robot_arm.go(True) ''' def go_to_designated_coordinate(self): desired_goal_pose = [-0.537,0.166, 0.248] Cplanning_frame = self.robot_arm.get_planning_frame() print(">> current planning frame: \n",Cplanning_frame) self.calculated_tomato_coor.position.x = desired_goal_pose[0] self.calculated_tomato_coor.position.y = desired_goal_pose[1] self.calculated_tomato_coor.position.z = desired_goal_pose[2] self.calculated_tomato_coor.orientation = Quaternion(*quaternion_from_euler(desired_goal_pose[0],desired_goal_pose[1],desired_goal_pose[2])) print(">> goal frame", self.calculated_tomato_coor) self.robot_arm.set_pose_target(self.calculated_tomato_coor) tf_display_position = [self.calculated_tomato_coor.position.x, self.calculated_tomato_coor.position.y, self.calculated_tomato_coor.position.z] tf_display_orientation = [self.calculated_tomato_coor.orientation.x, self.calculated_tomato_coor.orientation.y, self.calculated_tomato_coor.orientation.z, self.calculated_tomato_coor.orientation.w] jj = 0 while jj < 5: jj += 1 self.br.sendTransform( tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato_coor)) (plan, fraction) = self.robot_arm.compute_cartesian_path(tomato_waypoints,0.01,0.0) self.display_trajectory(plan) print("=========== Press `Enter` to if plan is correct!!...") raw_input() self.robot_arm.go(True) ''' def plan_cartesian_path(self, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x -= scale * 0.1 self.wpose.position.y += scale * 0.1 waypoints.append(copy.deepcopy(self.wpose)) ''' self.wpose.position.x -= scale*0.2 waypoints.append(copy.deepcopy(self.wpose)) ''' ''' self.wpose.position.x -= scale*0.1 waypoints.append(copy.deepcopy(self.wpose)) ''' (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def collectJsk(self, msg): global goal_x global goal_y global goal_z global ori_w global ori_x global ori_y global ori_z try: (trans, rot) = self.listener.lookupTransform('base_link', 'yolo_output00', rospy.Time(0)) goal_x = trans[0] goal_y = trans[1] goal_z = trans[2] ori_w = rot[0] ori_x = rot[1] ori_y = rot[2] ori_z = rot[3] print("==== trans[x,y,z]: ", trans) print("==== rot[x,y,z,w]: ", rot) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn('there is no tf')
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
class BaseEnvironment(): def __init__(self, limb_name): rospy.init_node('Baxter_Env') self.robot = RobotCommander() self.scene = PlanningSceneInterface() self.scene._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=0) self.limb_name = limb_name self.limb = baxter_interface.Limb(limb_name) self.group = moveit_commander.MoveGroupCommander(limb_name + "_arm") self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', DisplayTrajectory, queue_size=0) rospy.sleep(2) print("Initialized Base environment.") def construct_robot_state(self, joint): rs = RobotState() robot_state = self.robot.get_current_state() rs.joint_state.name = robot_state.joint_state.name rs.joint_state.position = list( robot_state.joint_state.position ) # filler for rest of the joint angles not found in waypoint joint_name_indices = [ rs.joint_state.name.index(n) for n in waypoint.keys() ] for i, idx in enumerate(joint_name_indices): rs.joint_state.position[idx] = waypoint.values()[i] return rs def move_to_random_state(self): waypoint = sample_loc(1, self.limb_name) self.limb.move_to_joint_positions(waypoint) # Baxter SDK; print("Moving robot arms to waypoint: %s" % (waypoint, )) def move_to_goal(self, goal): # Move the limb to goal; self.group.set_joint_value_target(goal) result = self.group.go() print "============ Waiting while moving robot to goal." print "============ Printing result after moving: %s" % result def plan_trajectory_in_cspace(self, goal, visualization=True): ## Then, we will get the current set of joint values for the group self.group.set_joint_value_target(goal) plan = self.group.plan() if visualization: print "============ Visualizing trajectory_plan" display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state( ) display_trajectory.trajectory.append(plan) self.display_trajectory_publisher.publish(display_trajectory) print "============ Waiting while plan is visualized (again)..." rospy.sleep(5) return plan
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move',anonymous=True) rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) #self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.marker = [] self.position_list = [] self.orientation_list = [] self.m_idd = 0 self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.ar_pose = Pose() self.goalPoseFromAR = Pose() self.br = tf.TransformBroadcaster() #self.goalPose_from_arPose = Pose() self.trans = [] self.rot = [] self.calculed_coke_pose = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): self.clear_octomap() planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print"====== current pose : ", self.wpose marker_joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) def look_object(self): look_object = self.robot_arm.get_current_joint_values() ## wrist3 현재 상태가 0도 인지, 360도인지에 따라 90도의 움직임을 -로 할지, + 할지 고려함 print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5] #look_object[5] = math.radians(90) look_object[5] = 0 ''' if look_object[5] > abs(math.radians(180)): if look_object[5] < 0: look_object[5] = look_object[5] + math.radians(90) # wrist3 elif look_object[5] > 0: look_object[5] = look_object[5] - math.radians(90) else: look_object[5] = look_object[5] - math.radians(90) # wrist3 ''' print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5] #look_object[3] = look_object[3] - (math.radians(00)) # wrist1 self.robot_arm.go(look_object, wait=True) #look_object[5] = look_object[5] + (math.radians(90)) # wrist3 #self.robot_arm.go(look_object, wait=True) def look_up_down(self): self.clear_octomap() print "======== clear_octomap... Please wait...." look_up_down = self.robot_arm.get_current_joint_values() #print "before: ", look_up_down look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory); def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose print "===== robot arm pose: ", self.wpose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10 #print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # waypoints.append(copy.deepcopy(self.wpose)) ii += 1 print "waypoints:", waypoints (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def print_ar_pose(self): rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") print "======= pos(meter): ", self.position_list print "======= orientation: ", self.orientation_list def go_to_move(self, scale = 1.0): # 로봇 팔: 마커 밑에 있는 물체 쪽으로 움직이기 #self.calculed_coke_pose = self.robot_arm.get_current_pose() planning_frame = self.robot_arm.get_planning_frame() coke_offset = [0, -0.35, -0.1] #x y z # gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품. # linear offset = abs([0, 0.0625, 0.13]) robot_base_offset = 0.873 base_wrist2_offset = 0.1 #for avoiding link contact error print "========== robot arm plannig frame: \n", planning_frame self.calculed_coke_pose.position.x = (scale * self.goal_x) + base_wrist2_offset # base_link to wrist2 x-offset self.calculed_coke_pose.position.y = (scale * self.goal_y) + coke_offset[1] #self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset self.calculed_coke_pose.position.z = (scale * self.goal_z) + robot_base_offset # world to base_link z-offset and coke can offset self.calculed_coke_pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.54)) print "========== coke_pose goal frame: ", self.calculed_coke_pose self.robot_arm.set_pose_target(self.calculed_coke_pose) tf_display_position = [self.calculed_coke_pose.position.x, self.calculed_coke_pose.position.y, self.calculed_coke_pose.position.z] tf_display_orientation = [self.calculed_coke_pose.orientation.x, self.calculed_coke_pose.orientation.y, self.calculed_coke_pose.orientation.z, self.calculed_coke_pose.orientation.w] ii = 0 while ii < 5: ii += 1 self.br.sendTransform( tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz coke_waypoints = [] coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose)) (coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path(coke_waypoints, 0.01, 0.0) self.display_trajectory(coke_plan) ## ## ## print "============ Press `Enter` to if plan is correct!! ..." raw_input() self.robot_arm.go(True) def ar_tf_listener(self, msg): try: self.marker = msg.markers ml = len(self.marker) target_id = 9 #self.m_idd = self.marker[0].id # 임시용 for ii in range(0, ml): # 0 <= ii < ml self.m_idd = self.marker[ii].id #print "checked all id: ", self.m_idd if self.m_idd != target_id: pass #target_id_flage = False elif self.m_idd == target_id: target_id_flage = True target_id = self.m_idd target_id_index = ii #print "target id: ", target_id_index, target_id, target_id_flage if target_id_flage == True: self.ar_pose.position.x = self.marker[target_id_index].pose.pose.position.x self.ar_pose.position.y = self.marker[target_id_index].pose.pose.position.y self.ar_pose.position.z = self.marker[target_id_index].pose.pose.position.z self.ar_pose.orientation.x = self.marker[target_id_index].pose.pose.orientation.x self.ar_pose.orientation.y = self.marker[target_id_index].pose.pose.orientation.y self.ar_pose.orientation.z = self.marker[target_id_index].pose.pose.orientation.z self.ar_pose.orientation.w = self.marker[target_id_index].pose.pose.orientation.w self.goal_x = self.ar_pose.position.x self.goal_y = self.ar_pose.position.y self.goal_z = self.ar_pose.position.z self.position_list = [self.goal_x, self.goal_y, self.goal_z] self.orientation_list = [self.ar_pose.orientation.x, self.ar_pose.orientation.y, self.ar_pose.orientation.z, self.ar_pose.orientation.w] (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.orientation_list) #list form으로 넘겨주어야 함 #print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z #print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw #print "ar_pos(meter): \n", self.position_list #print "ar_orientation: \n", self.orientation_list except: return
) # initializes rospy node, creates a test environment with collision objects """ Preliminaries, initialize limb, set up publisher for collision checking """ limb_name = 'right' limb = baxter_interface.Limb(limb_name) robot_state_collision_pub = rospy.Publisher('/robot_collision_state', DisplayRobotState, queue_size=0) sv = StateValidity() # Sample a waypoint waypoint = sample_loc(num_samples=1, name=limb_name) # CONSTRUCT A ROBOTSTATE MSG FROM SAMPLED JOINT FOR COLLISION CHECKING rs = RobotState() robot = RobotCommander() robot_state = robot.get_current_state() rs.joint_state.name = robot_state.joint_state.name rs.joint_state.position = list( robot_state.joint_state.position ) # filler for rest of the joint angles not found in waypoint joint_name_indices = [ rs.joint_state.name.index(n) for n in waypoint.keys() ] for i, idx in enumerate(joint_name_indices): rs.joint_state.position[idx] = waypoint.values()[i] collision_flag = sv.getStateValidity(rs, group_name=limb_name + '_arm') print(collision_flag) # Boolean limb.move_to_joint_positions( waypoint) # moves to waypoint for visual confirmation
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.detected_names = rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') self.object_pose_sub = rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk) self.listener = tf.TransformListener() # self.object_name_sub = rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect) self.tomatoboundingBox = BoundingBox() display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20) global goal_x global goal_y global goal_z global goal_roll global goal_pitch global goal_yaw self.distance = 0 self.trans = [0.0, 0.0, 0.0] # self.marker = [] self.tomato = [] self.position_list = [] self.orientation_list = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print ("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print ("====== current pose : ", self.wpose) joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982] print ("INIT POSE: ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait = True) def go_to_desired_coordinate(self): self.calculated_tomato.position.x = goal_x self.calculated_tomato.position.y = goal_y self.calculated_tomato.position.z = goal_z self.calculated_tomato.orientation.x = goal_roll self.calculated_tomato.orientation.y = goal_pitch self.calculated_tomato.orientation.z = goal_yaw tf_display_position = [self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z] tf_display_orientation = [self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w] ii = 0 while ii < 5: ii += 1 self.br.sendTransform( tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz tomato_id_goal_waypoints = [] tomato_id_goal_waypoints.append(copy.deepcopy(calculated_tomato)) (tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(tomato_id_goal_waypoints, 0.01, 0.0) self.display_trajectory(tomato_id_goal_plan) print ("============ Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_x(self, x_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_y(self, y_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_z(self, z_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk) # rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect) def collectJsk(self, msg): global goal_x global goal_y global goal_z global goal_roll global goal_pitch global goal_yaw try: (trans, rot) = self.listener.lookupTransform('base_link','yolo_output00', rospy.Time(0)) goal_x = round(trans[0],2) goal_y = round(trans[1],2) goal_z = round(trans[2],2) print("====== trans [x, y, z]: ", trans) print("====== rotat [x, y, z, w]: ", rot) orientation = euler_from_quaternion(rot) goal_roll = orientation[0] goal_pitch = orientation[1] goal_yaw = orientation[2] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn('there is no tf')
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")
class BaxterMoveit(BaxterRobot): LEFT = -1 RIGHT = 1 def __init__(self, node, limb=+1): super(BaxterMoveit, self).__init__(node, limb=+1) self.group = MoveGroupCommander(self.lns + "_arm") self.robot = RobotCommander() self.scene = PlanningSceneInterface() self._info() def _info(self): '''Getting Basic Information''' # name of the reference frame for this robot: print("@@@@@@@@@@@@ Reference frame: %s" % self.group.get_planning_frame()) # We can also print the name of the end-effector link for this group: print("@@@@@@@@@@@@ End effector: %s" % self.group.get_end_effector_link()) # We can get a list of all the groups in the robot: print("@@@@@@@@@@@@ Robot Groups: @@@@@@@@@@@@@", self.robot.get_group_names()) # Sometimes for debugging it is useful to print the entire state of the # robot: print("@@@@@@@@@@@@ Printing robot state @@@@@@@@@@@@@") print(self.robot.get_current_state()) def obstacle(self, name, position, size): planning_frame = self.robot.get_planning_frame() pose = PoseStamped() pose.header.frame_id = planning_frame pose.pose.position.x = position[0] pose.pose.position.y = position[1] pose.pose.position.z = position[2] self.scene.add_box(name, pose, tuple(size)) def movej(self, q, raw=False): ''' move in joint space by giving a joint configuration as dictionary''' if raw: print("in moveit 'raw' motion is not avaiable") # succ = False # while succ is False: succ = self.group.go(q, wait=True) # self.group.stop() # ensures that there is no residual movement def showplan(self, target): if type(target) is PyKDL.Frame or type(target) is Pose: q = self.ik(target) elif type(target) is dict: q = target else: print("Target format error") return self.group.set_joint_value_target(q) self.group.plan() def movep(self, pose, raw=False): ''' move the eef in Cartesian space by giving a geometry_msgs.Pose or a PyKDL.Frame''' if type(pose) is PyKDL.Frame: pose = transformations.KDLToPose(pose) q = self.ik(pose) if q is not None: self.movej(q, raw=raw) else: print("\nNO SOLUTION TO IK\n" * 20)
def simple_function(): rc = RobotCommander() mgc = MoveGroupCommander("manipulator") # print(rc.get_group_names()) # print(rc.get_group('manipulator')) # exit() eef_step = 0.01 jump_threshold = 2 ### Create a handle for the Planning Scene Interface psi = PlanningSceneInterface() rc.get_current_state() rospy.logwarn(rc.get_current_state()) sss.wait_for_input() #rate = rospy.Rate(0.1) # 10hz rate = rospy.Rate(1) # 10hz rospy.sleep(1) theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1) while not rospy.is_shutdown(): approach_pose = PoseStamped() approach_pose.header.frame_id = "table" approach_pose.header.stamp = rospy.Time(0) approach_pose.pose.position.x = 0.146 approach_pose.pose.position.y = 0.622 approach_pose.pose.position.z = 0.01 quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2) approach_pose.pose.orientation.x = quat[0] approach_pose.pose.orientation.y = quat[1] approach_pose.pose.orientation.z = quat[2] approach_pose.pose.orientation.w = quat[3] # mgc.set_start_state_to_current_state() # (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True) # if(frac_approach != 1): # rospy.logwarn("Planning did not succeed before adding collision objects") # else: # rospy.logwarn("Planning succeeded before adding collision objects") # # rospy.logwarn("waiting for input to add dummy box") # sss.wait_for_input() # box_dummy_pose = PoseStamped() box_dummy_pose.header.frame_id = "table" box_dummy_pose.pose.position.x = 0.147 box_dummy_pose.pose.position.y = 0.636 box_dummy_pose.pose.position.z = 0 psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC # rospy.logwarn("I have added the box") # rospy.sleep(1) # rospy.logwarn("waiting for input to try planning with dummy box") # sss.wait_for_input() # # (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True) # if(frac_approach != 1): # rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)") # else: # rospy.logwarn("Planning succeeded after adding collision objects (dummy box)") # rospy.logwarn("waiting for input to add end effector box box") sss.wait_for_input() # end effector collision object link_attached_to_ef = "ee_link" mb_ef_collisonobj = "metal_bracket" mb_ef_pose = PoseStamped() mb_ef_pose.header.frame_id = link_attached_to_ef mb_ef_pose.pose.position.x = 0.065/2.0 mb_ef_pose.pose.position.y = 0.0 mb_ef_pose.pose.position.z = 0.0 mb_ef_size = (0.065,0.06,0.06) psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"]) raw_input("0 hi") mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"]) rospy.logwarn("I have added the attached box to the end effector") rospy.sleep(1) raw_input("1 hi") rospy.logwarn(rc.get_current_state()) # mgc.attach_object(object_name, link_name, touch_links) mgc.set_start_state_to_current_state() rospy.logwarn(rc.get_current_state()) raw_input("2 hi") rospy.logwarn("waiting for input to try planning with end effector box") sss.wait_for_input() (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True) if(frac_approach != 1): rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)") else: rospy.logwarn("Planning succeeded after adding collision objects (dummy box)") rospy.logwarn("waiting for input to try planning next loop") sss.wait_for_input() rate.sleep()