def compute_cartesian_path_async(self, callback, errback, frames, base_link, ee_link, group, joint_names, joint_positions, max_step, avoid_collisions, path_constraints, attached_collision_object): """ """ header = Header(frame_id=base_link) waypoints = [Pose.from_frame(frame) for frame in frames] joint_state = JointState(header=header, name=joint_names, position=joint_positions) start_state = RobotState(joint_state, MultiDOFJointState(header=header)) if attached_collision_object: start_state.attached_collision_objects = [ attached_collision_object ] request = dict(header=header, start_state=start_state, group_name=group, link_name=ee_link, waypoints=waypoints, max_step=float(max_step), avoid_collisions=bool(avoid_collisions), path_constraints=path_constraints) self.GET_CARTESIAN_PATH(self, request, callback, errback)
def motion_plan_goal_joint_positions_async( self, callback, errback, joint_positions_goal, joint_names_goal, tolerances, base_link, group, joint_names, joint_positions, path_constraints=None, trajectory_constraints=None, planner_id='', num_planning_attempts=8, allowed_planning_time=2., max_velocity_scaling_factor=1., max_acceleration_scaling_factor=1., attached_collision_object=None): """ """ # http://docs.ros.org/jade/api/moveit_core/html/utils_8cpp_source.html header = Header(frame_id=base_link) joint_state = JointState(header=header, name=joint_names, position=joint_positions) start_state = RobotState(joint_state, MultiDOFJointState(header=header)) if attached_collision_object: start_state.attached_collision_objects = [ attached_collision_object ] joint_constraints = [] for position, joint_name, tolerance in zip(joint_positions_goal, joint_names_goal, tolerances): jcm = JointConstraint(joint_name, position, tolerance, tolerance) joint_constraints.append(jcm) # TODO: possibility to hand over more goal constraints goal_constraints = [Constraints(joint_constraints=joint_constraints)] request = dict( start_state=start_state, goal_constraints=goal_constraints, path_constraints=path_constraints, trajectory_constraints=trajectory_constraints, planner_id=planner_id, group_name=group, num_planning_attempts=num_planning_attempts, allowed_planning_time=allowed_planning_time, max_velocity_scaling_factor=max_velocity_scaling_factor, max_acceleration_scaling_factor=max_velocity_scaling_factor) self.GET_MOTION_PLAN(self, request, callback, errback)
def motion_plan_goal_frame_async(self, callback, errback, frame, base_link, ee_link, group, joint_names, joint_positions, tolerance_position, tolerance_angle, path_constraints=None, trajectory_constraints=None, planner_id='', num_planning_attempts=8, allowed_planning_time=2., max_velocity_scaling_factor=1., max_acceleration_scaling_factor=1., attached_collision_object=None): """ """ # http://docs.ros.org/jade/api/moveit_core/html/utils_8cpp_source.html # TODO: if list of frames (goals) => receive multiple solutions? header = Header(frame_id=base_link) joint_state = JointState(header=header, name=joint_names, position=joint_positions) start_state = RobotState(joint_state, MultiDOFJointState(header=header)) if attached_collision_object: start_state.attached_collision_objects = [ attached_collision_object ] pose = Pose.from_frame(frame) pcm = PositionConstraint(header=header, link_name=ee_link) pcm.target_point_offset.x = 0. pcm.target_point_offset.y = 0. pcm.target_point_offset.z = 0. bv = SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[tolerance_position]) pcm.constraint_region.primitives = [bv] pcm.constraint_region.primitive_poses = [ Pose(pose.position, Quaternion(0, 0, 0, 1)) ] ocm = OrientationConstraint(header=header, link_name=ee_link) ocm.orientation = Quaternion.from_frame(frame) ocm.absolute_x_axis_tolerance = tolerance_angle ocm.absolute_y_axis_tolerance = tolerance_angle ocm.absolute_z_axis_tolerance = tolerance_angle # TODO: possibility to hand over more goal constraints goal_constraints = [ Constraints(position_constraints=[pcm], orientation_constraints=[ocm]) ] request = dict( start_state=start_state, goal_constraints=goal_constraints, path_constraints=path_constraints, trajectory_constraints=trajectory_constraints, planner_id=planner_id, group_name=group, num_planning_attempts=num_planning_attempts, allowed_planning_time=allowed_planning_time, max_velocity_scaling_factor=max_velocity_scaling_factor, max_acceleration_scaling_factor=max_velocity_scaling_factor) self.GET_MOTION_PLAN(self, request, callback, errback)