def to_shape_msg(self):
"""
Creates an instance of moveit/shape_msgs.Plane for a plane primitive
or an instance of moveit/shape_msgs.SolidPrimitive for all other primitive
"""
if self.__kind is CollisionPrimitiveKind.plane:
msg = shape_msgs.Plane()
msg.coef = list(self.__parameters)
else:
msg = shape_msgs.SolidPrimitive()
msg.type = self.__kind.value
msg.dimensions = list(self.__parameters)
return msg
def _to_pose_constraint(pose, reference_frame, link_name, position_tolerance=_DEFAULT_POSITION_TOLERANCE):
"""Returns an position constraint suitable for ActionGoal's."""
pos_con = PositionConstraint()
pos_con.header.frame_id = reference_frame
pos_con.link_name = link_name
pos_con.constraint_region.primitive_poses.append(pose)
pos_con.weight = 1
region = shape_msgs.SolidPrimitive()
region.type = shape_msgs.SolidPrimitive.SPHERE
region.dimensions.append(position_tolerance)
pos_con.constraint_region.primitives.append(region)
return pos_con
def BasicMicoEnvironment():
## all objects in one message
env_objects = moveit_msgs.CollisionObject()
env_objects.header.stamp = rospy.Time.now()
env_objects.header.frame_id = "/world"
env_objects.id = "table_camera_user"
#table
# table = shape_msgs.SolidPrimitive()
# table.type = shape_msgs.SolidPrimitive.BOX
# table.dimensions.append(2.0)
# y = 1.2
# table.dimensions.append(y)
# table.dimensions.append(0.05)
# table_pose = geometry_msgs.Pose()
# table_pose.position.y = - y/2.0
# table_pose.position.z = - 0.03
# table_pose.orientation.w = 1.0
#camera
camera = shape_msgs.SolidPrimitive()
camera.type = shape_msgs.SolidPrimitive.BOX
camera.dimensions.append(0.3)
camera.dimensions.append(0.08)
camera.dimensions.append(0.06)
camera_pose = geometry_msgs.Pose()
camera_pose.position.x = 0.581
camera_pose.position.y = 0.065
camera_pose.position.z = 0.05 / 2.0 + 0.376
camera_pose.orientation.z = 0.3827
camera_pose.orientation.w = -0.9239
#camera pole
camera_pole = shape_msgs.SolidPrimitive()
camera_pole.type = shape_msgs.SolidPrimitive.CYLINDER
camera_pole.dimensions.append(0.376) #height
camera_pole.dimensions.append(0.02) #radius
camera_pole_pose = geometry_msgs.Pose()
camera_pole_pose.position.x = 0.581
camera_pole_pose.position.y = 0.065
camera_pole_pose.position.z = 0.376 / 2.0
camera_pole_pose.orientation.w = 1.0
#user area
user_area = shape_msgs.SolidPrimitive()
user_area.type = shape_msgs.SolidPrimitive.BOX
user_area.dimensions.append(1.2)
user_area.dimensions.append(0.05)
user_area.dimensions.append(0.6)
user_area_pose = geometry_msgs.Pose()
user_area_pose.position.x = 0.0
user_area_pose.position.y = 0.3
user_area_pose.position.z = 0.3
user_area_pose.orientation.w = 1.0
# env_objects.primitives.append(table) #one can also append more objects under the same id
env_objects.primitives.append(camera)
env_objects.primitives.append(camera_pole)
env_objects.primitives.append(user_area)
# env_objects.primitive_poses.append(table_pose)
env_objects.primitive_poses.append(camera_pose)
env_objects.primitive_poses.append(camera_pole_pose)
env_objects.primitive_poses.append(user_area_pose)
env_objects.operation = moveit_msgs.CollisionObject.ADD
pub_coll_obj = rospy.Publisher('/collision_object',
moveit_msgs.CollisionObject,
queue_size=1)
count = 0
rate = rospy.Rate(50)
while not rospy.is_shutdown() and count < 100:
pub_coll_obj.publish(env_objects)
count += 1
rate.sleep()
def generate_planning_goal(self, joint_state, tip_pose, target_pose):
# create the motion plan request
plan_req = moveit_msgs.MotionPlanRequest()
# defint the motion planning workspace
for wp in [plan_req.workspace_parameters]:
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = self.root_link
# TODO: this is robot-specific
for c in [wp.min_corner]:
c.x = -1.5
c.y = -1.5
c.z = 0.0
for c in [wp.max_corner]:
c.x = 1.5
c.y = 1.5
c.z = 1.5
# configure the planning options
plan_req.allowed_planning_time = 0.5
plan_req.group_name = 'arm'
#plan_req.planner_id = 'SBLkConfigDefault'
plan_req.planner_id = 'RRTConnectkConfigDefault'
plan_req.start_state.is_diff = False
# add the goal constraints
plan_req.goal_constraints.append(moveit_msgs.Constraints())
# configure the goal constraints
for goal_constraints in [plan_req.goal_constraints[0]]:
# define the name and add a position and orientation constraint
goal_constraints.name = 'goal'
goal_constraints.position_constraints.append(
moveit_msgs.PositionConstraint())
goal_constraints.orientation_constraints.append(
moveit_msgs.OrientationConstraint())
# create the position goal constraint
for g in [goal_constraints.position_constraints[0]]:
g.header.stamp = rospy.Time.now()
g.header.frame_id = self.root_link
g.link_name = self.tip_link
g.weight = 1.0
for tpo in [g.target_point_offset]:
tpo.x = 0.0
tpo.y = 0.0
tpo.z = 0.0
for cr in [g.constraint_region]:
cr.primitives.append(shape_msgs.SolidPrimitive())
cr.primitives[0].type = shape_msgs.SolidPrimitive.SPHERE
cr.primitives[0].dimensions = [0.005]
# set the target position
cr.primitive_poses = [toMsg(fromTf(target_pose))]
# create the orientation goal constraint
for g in [goal_constraints.orientation_constraints[0]]:
g.header.stamp = rospy.Time.now()
g.header.frame_id = self.root_link
g.link_name = self.tip_link
g.absolute_x_axis_tolerance = 0.005
g.absolute_y_axis_tolerance = 0.005
g.absolute_z_axis_tolerance = 0.005
g.weight = 1.0
# set the target orientation
g.orientation = toMsg(fromTf(target_pose)).orientation
# Set the start state
plan_req.start_state.joint_state = joint_state
plan_opts = moveit_msgs.PlanningOptions()
plan_opts.plan_only = True
return moveit_msgs.MoveGroupGoal(request=plan_req,
planning_options=plan_opts)
def rave_env_to_ros(env):
pub = make_planning_scene_pub()
ps = mm.PlanningScene()
with open(osp.join(pbc.miscdata_dir, "planning_scene_prototype.yaml"),
"r") as fh:
d = yaml.load(fh)
rm.fill_message_args(ps, d)
cos = ps.world.collision_objects
for body in env.GetBodies():
if body.IsRobot():
rstate = ps.robot_state
rave_joint_vals = body.GetDOFValues()
rave_joint_names = [joint.GetName() for joint in body.GetJoints()]
ros_joint_names = []
ros_joint_values = []
for name in ROS_JOINT_NAMES:
if name in rave_joint_names:
i = rave_joint_names.index(name)
ros_joint_values.append(rave_joint_vals[i])
ros_joint_names.append(name)
rstate.joint_state.position = ros_joint_values
rstate.joint_state.name = ros_joint_names
rstate.multi_dof_joint_state.header.frame_id = 'odom_combined'
rstate.multi_dof_joint_state.header.stamp = rospy.Time.now()
rstate.multi_dof_joint_state.joint_names = ['world_joint']
rstate.multi_dof_joint_state.joint_transforms = [
RosTransformFromRaveMatrix(body.GetTransform())
]
else:
for link in body.GetLinks():
co = mm.CollisionObject()
co.operation = co.ADD
co.id = body.GetName()
co.header.frame_id = "odom_combined"
co.header.stamp = rospy.Time.now()
for geom in link.GetGeometries():
trans = RosPoseFromRaveMatrix(body.GetTransform().dot(
link.GetTransform().dot(geom.GetTransform())))
if geom.GetType() == openravepy.GeometryType.Trimesh:
mesh = sm.Mesh()
rave_mesh = geom.GetCollisionMesh()
for pt in rave_mesh.vertices:
mesh.vertices.append(gm.Point(*pt))
for tri in rave_mesh.indices:
mt = sm.MeshTriangle()
mt.vertex_indices = tri
mesh.triangles.append(mt)
co.meshes.append(mesh)
co.mesh_poses.append(trans)
else:
shape = sm.SolidPrimitive()
shape.type = shape.BOX
shape.dimensions = geom.GetBoxExtents() * 2
co.primitives.append(shape)
co.primitive_poses.append(trans)
cos.append(co)
pub.publish(ps)
return ps
