def build_collision_object(self, frame_id, id_name, compas_mesh, operation): co = CollisionObject(header=Header(frame_id=frame_id), id=id_name) if compas_mesh: # ROS mesh message requires triangles mesh_quads_to_triangles(compas_mesh) mesh = Mesh.from_mesh(compas_mesh) co.meshes = [mesh] co.mesh_poses = [Pose()] if operation == 0: co.operation = CollisionObject.ADD elif operation == 1: co.operation = CollisionObject.REMOVE elif operation == 2: co.operation = CollisionObject.APPEND else: raise ValueError("Operation unknown") return co
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)