00001 """autogenerated by genmsg_py from GetConstraintAwarePositionIKRequest.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import kinematics_msgs.msg
00006 import roslib.rostime
00007 import geometric_shapes_msgs.msg
00008 import geometry_msgs.msg
00009 import sensor_msgs.msg
00010 import motion_planning_msgs.msg
00011 import std_msgs.msg
00012
00013 class GetConstraintAwarePositionIKRequest(roslib.message.Message):
00014 _md5sum = "f6219d12eb11bca3fc61530da7850970"
00015 _type = "kinematics_msgs/GetConstraintAwarePositionIKRequest"
00016 _has_header = False
00017 _full_text = """
00018
00019 kinematics_msgs/PositionIKRequest ik_request
00020
00021 motion_planning_msgs/AllowedContactSpecification[] allowed_contacts
00022
00023 motion_planning_msgs/OrderedCollisionOperations ordered_collision_operations
00024
00025 motion_planning_msgs/LinkPadding[] link_padding
00026
00027 motion_planning_msgs/Constraints constraints
00028
00029 duration timeout
00030
00031 ================================================================================
00032 MSG: kinematics_msgs/PositionIKRequest
00033 # A Position IK request message
00034 # The name of the link for which we are computing IK
00035 string ik_link_name
00036
00037 # The (stamped) pose of the link
00038 geometry_msgs/PoseStamped pose_stamped
00039
00040 # A RobotState consisting of hint/seed positions for the IK computation.
00041 # These may be used to seed the IK search.
00042 # The seed state MUST contain state for all joints to be used by the IK solver
00043 # to compute IK. The list of joints that the IK solver deals with can be found using
00044 # the kinematics_msgs/GetKinematicSolverInfo
00045 motion_planning_msgs/RobotState ik_seed_state
00046
00047 # Additional state information can be provided here to specify the starting positions
00048 # of other joints/links on the robot.
00049 motion_planning_msgs/RobotState robot_state
00050
00051 ================================================================================
00052 MSG: geometry_msgs/PoseStamped
00053 # A Pose with reference coordinate frame and timestamp
00054 Header header
00055 Pose pose
00056
00057 ================================================================================
00058 MSG: std_msgs/Header
00059 # Standard metadata for higher-level stamped data types.
00060 # This is generally used to communicate timestamped data
00061 # in a particular coordinate frame.
00062 #
00063 # sequence ID: consecutively increasing ID
00064 uint32 seq
00065 #Two-integer timestamp that is expressed as:
00066 # * stamp.secs: seconds (stamp_secs) since epoch
00067 # * stamp.nsecs: nanoseconds since stamp_secs
00068 # time-handling sugar is provided by the client library
00069 time stamp
00070 #Frame this data is associated with
00071 # 0: no frame
00072 # 1: global frame
00073 string frame_id
00074
00075 ================================================================================
00076 MSG: geometry_msgs/Pose
00077 # A representation of pose in free space, composed of postion and orientation.
00078 Point position
00079 Quaternion orientation
00080
00081 ================================================================================
00082 MSG: geometry_msgs/Point
00083 # This contains the position of a point in free space
00084 float64 x
00085 float64 y
00086 float64 z
00087
00088 ================================================================================
00089 MSG: geometry_msgs/Quaternion
00090 # This represents an orientation in free space in quaternion form.
00091
00092 float64 x
00093 float64 y
00094 float64 z
00095 float64 w
00096
00097 ================================================================================
00098 MSG: motion_planning_msgs/RobotState
00099 # This message contains information about the robot state, i.e. the positions of its joints and links
00100 sensor_msgs/JointState joint_state
00101 motion_planning_msgs/MultiDOFJointState multi_dof_joint_state
00102 ================================================================================
00103 MSG: sensor_msgs/JointState
00104 # This is a message that holds data to describe the state of a set of torque controlled joints.
00105 #
00106 # The state of each joint (revolute or prismatic) is defined by:
00107 # * the position of the joint (rad or m),
00108 # * the velocity of the joint (rad/s or m/s) and
00109 # * the effort that is applied in the joint (Nm or N).
00110 #
00111 # Each joint is uniquely identified by its name
00112 # The header specifies the time at which the joint states were recorded. All the joint states
00113 # in one message have to be recorded at the same time.
00114 #
00115 # This message consists of a multiple arrays, one for each part of the joint state.
00116 # The goal is to make each of the fields optional. When e.g. your joints have no
00117 # effort associated with them, you can leave the effort array empty.
00118 #
00119 # All arrays in this message should have the same size, or be empty.
00120 # This is the only way to uniquely associate the joint name with the correct
00121 # states.
00122
00123
00124 Header header
00125
00126 string[] name
00127 float64[] position
00128 float64[] velocity
00129 float64[] effort
00130
00131 ================================================================================
00132 MSG: motion_planning_msgs/MultiDOFJointState
00133 #A representation of a multi-dof joint state
00134 time stamp
00135 string[] joint_names
00136 string[] frame_ids
00137 string[] child_frame_ids
00138 geometry_msgs/Pose[] poses
00139
00140 ================================================================================
00141 MSG: motion_planning_msgs/AllowedContactSpecification
00142 # The names of the regions
00143 string name
00144
00145 # The shape of the region in the environment
00146 geometric_shapes_msgs/Shape shape
00147
00148 # The pose of the space defining the region
00149 geometry_msgs/PoseStamped pose_stamped
00150
00151 # The set of links that will be allowed to have penetration contact within this region
00152 string[] link_names
00153
00154 # The maximum penetration depth allowed for every link
00155 float64 penetration_depth
00156 ================================================================================
00157 MSG: geometric_shapes_msgs/Shape
00158 byte SPHERE=0
00159 byte BOX=1
00160 byte CYLINDER=2
00161 byte MESH=3
00162
00163 byte type
00164
00165
00166 #### define sphere, box, cylinder ####
00167 # the origin of each shape is considered at the shape's center
00168
00169 # for sphere
00170 # radius := dimensions[0]
00171
00172 # for cylinder
00173 # radius := dimensions[0]
00174 # length := dimensions[1]
00175 # the length is along the Z axis
00176
00177 # for box
00178 # size_x := dimensions[0]
00179 # size_y := dimensions[1]
00180 # size_z := dimensions[2]
00181 float64[] dimensions
00182
00183
00184 #### define mesh ####
00185
00186 # list of triangles; triangle k is defined by tre vertices located
00187 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00188 int32[] triangles
00189 geometry_msgs/Point[] vertices
00190
00191 ================================================================================
00192 MSG: motion_planning_msgs/OrderedCollisionOperations
00193 # A set of collision operations that will be performed in the order they are specified
00194 CollisionOperation[] collision_operations
00195 ================================================================================
00196 MSG: motion_planning_msgs/CollisionOperation
00197 # A definition of a collision operation
00198 # E.g. ("gripper",COLLISION_SET_ALL,ENABLE) will enable collisions
00199 # between the gripper and all objects in the collision space
00200
00201 string object1
00202 string object2
00203 string COLLISION_SET_ALL="all"
00204 string COLLISION_SET_OBJECTS="objects"
00205 string COLLISION_SET_ATTACHED_OBJECTS="attached"
00206
00207 # The penetration distance to which collisions are allowed. This is 0.0 by default.
00208 float64 penetration_distance
00209
00210 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above
00211 int32 operation
00212 int32 DISABLE=0
00213 int32 ENABLE=1
00214
00215 ================================================================================
00216 MSG: motion_planning_msgs/LinkPadding
00217 #name for the link
00218 string link_name
00219
00220 # padding to apply to the link
00221 float64 padding
00222
00223 ================================================================================
00224 MSG: motion_planning_msgs/Constraints
00225 # This message contains a list of motion planning constraints.
00226
00227 motion_planning_msgs/JointConstraint[] joint_constraints
00228 motion_planning_msgs/PositionConstraint[] position_constraints
00229 motion_planning_msgs/OrientationConstraint[] orientation_constraints
00230 motion_planning_msgs/VisibilityConstraint[] visibility_constraints
00231
00232 ================================================================================
00233 MSG: motion_planning_msgs/JointConstraint
00234 # Constrain the position of a joint to be within a certain bound
00235 string joint_name
00236
00237 # the bound to be achieved is [position - tolerance_below, position + tolerance_above]
00238 float64 position
00239 float64 tolerance_above
00240 float64 tolerance_below
00241
00242 # A weighting factor for this constraint
00243 float64 weight
00244 ================================================================================
00245 MSG: motion_planning_msgs/PositionConstraint
00246 # This message contains the definition of a position constraint.
00247 Header header
00248
00249 # The robot link this constraint refers to
00250 string link_name
00251
00252 # The offset (in the link frame) for the target point on the link we are planning for
00253 geometry_msgs/Point target_point_offset
00254
00255 # The nominal/target position for the point we are planning for
00256 geometry_msgs/Point position
00257
00258 # The shape of the bounded region that constrains the position of the end-effector
00259 # This region is always centered at the position defined above
00260 geometric_shapes_msgs/Shape constraint_region_shape
00261
00262 # The orientation of the bounded region that constrains the position of the end-effector.
00263 # This allows the specification of non-axis aligned constraints
00264 geometry_msgs/Quaternion constraint_region_orientation
00265
00266 # Constraint weighting factor - a weight for this constraint
00267 float64 weight
00268 ================================================================================
00269 MSG: motion_planning_msgs/OrientationConstraint
00270 # This message contains the definition of an orientation constraint.
00271 Header header
00272
00273 # The robot link this constraint refers to
00274 string link_name
00275
00276 # The type of the constraint
00277 int32 type
00278 int32 LINK_FRAME=0
00279 int32 HEADER_FRAME=1
00280
00281 # The desired orientation of the robot link specified as a quaternion
00282 geometry_msgs/Quaternion orientation
00283
00284 # optional RPY error tolerances specified if
00285 float64 absolute_roll_tolerance
00286 float64 absolute_pitch_tolerance
00287 float64 absolute_yaw_tolerance
00288
00289 # Constraint weighting factor - a weight for this constraint
00290 float64 weight
00291
00292 ================================================================================
00293 MSG: motion_planning_msgs/VisibilityConstraint
00294 # This message contains the definition of a visibility constraint.
00295 Header header
00296
00297 # The point stamped target that needs to be kept within view of the sensor
00298 geometry_msgs/PointStamped target
00299
00300 # The local pose of the frame in which visibility is to be maintained
00301 # The frame id should represent the robot link to which the sensor is attached
00302 # The visual axis of the sensor is assumed to be along the X axis of this frame
00303 geometry_msgs/PoseStamped sensor_pose
00304
00305 # The deviation (in radians) that will be tolerated
00306 # Constraint error will be measured as the solid angle between the
00307 # X axis of the frame defined above and the vector between the origin
00308 # of the frame defined above and the target location
00309 float64 absolute_tolerance
00310
00311
00312 ================================================================================
00313 MSG: geometry_msgs/PointStamped
00314 # This represents a Point with reference coordinate frame and timestamp
00315 Header header
00316 Point point
00317
00318 """
00319 __slots__ = ['ik_request','allowed_contacts','ordered_collision_operations','link_padding','constraints','timeout']
00320 _slot_types = ['kinematics_msgs/PositionIKRequest','motion_planning_msgs/AllowedContactSpecification[]','motion_planning_msgs/OrderedCollisionOperations','motion_planning_msgs/LinkPadding[]','motion_planning_msgs/Constraints','duration']
00321
00322 def __init__(self, *args, **kwds):
00323 """
00324 Constructor. Any message fields that are implicitly/explicitly
00325 set to None will be assigned a default value. The recommend
00326 use is keyword arguments as this is more robust to future message
00327 changes. You cannot mix in-order arguments and keyword arguments.
00328
00329 The available fields are:
00330 ik_request,allowed_contacts,ordered_collision_operations,link_padding,constraints,timeout
00331
00332 @param args: complete set of field values, in .msg order
00333 @param kwds: use keyword arguments corresponding to message field names
00334 to set specific fields.
00335 """
00336 if args or kwds:
00337 super(GetConstraintAwarePositionIKRequest, self).__init__(*args, **kwds)
00338
00339 if self.ik_request is None:
00340 self.ik_request = kinematics_msgs.msg.PositionIKRequest()
00341 if self.allowed_contacts is None:
00342 self.allowed_contacts = []
00343 if self.ordered_collision_operations is None:
00344 self.ordered_collision_operations = motion_planning_msgs.msg.OrderedCollisionOperations()
00345 if self.link_padding is None:
00346 self.link_padding = []
00347 if self.constraints is None:
00348 self.constraints = motion_planning_msgs.msg.Constraints()
00349 if self.timeout is None:
00350 self.timeout = roslib.rostime.Duration()
00351 else:
00352 self.ik_request = kinematics_msgs.msg.PositionIKRequest()
00353 self.allowed_contacts = []
00354 self.ordered_collision_operations = motion_planning_msgs.msg.OrderedCollisionOperations()
00355 self.link_padding = []
00356 self.constraints = motion_planning_msgs.msg.Constraints()
00357 self.timeout = roslib.rostime.Duration()
00358
00359 def _get_types(self):
00360 """
00361 internal API method
00362 """
00363 return self._slot_types
00364
00365 def serialize(self, buff):
00366 """
00367 serialize message into buffer
00368 @param buff: buffer
00369 @type buff: StringIO
00370 """
00371 try:
00372 _x = self.ik_request.ik_link_name
00373 length = len(_x)
00374 buff.write(struct.pack('<I%ss'%length, length, _x))
00375 _x = self
00376 buff.write(_struct_3I.pack(_x.ik_request.pose_stamped.header.seq, _x.ik_request.pose_stamped.header.stamp.secs, _x.ik_request.pose_stamped.header.stamp.nsecs))
00377 _x = self.ik_request.pose_stamped.header.frame_id
00378 length = len(_x)
00379 buff.write(struct.pack('<I%ss'%length, length, _x))
00380 _x = self
00381 buff.write(_struct_7d3I.pack(_x.ik_request.pose_stamped.pose.position.x, _x.ik_request.pose_stamped.pose.position.y, _x.ik_request.pose_stamped.pose.position.z, _x.ik_request.pose_stamped.pose.orientation.x, _x.ik_request.pose_stamped.pose.orientation.y, _x.ik_request.pose_stamped.pose.orientation.z, _x.ik_request.pose_stamped.pose.orientation.w, _x.ik_request.ik_seed_state.joint_state.header.seq, _x.ik_request.ik_seed_state.joint_state.header.stamp.secs, _x.ik_request.ik_seed_state.joint_state.header.stamp.nsecs))
00382 _x = self.ik_request.ik_seed_state.joint_state.header.frame_id
00383 length = len(_x)
00384 buff.write(struct.pack('<I%ss'%length, length, _x))
00385 length = len(self.ik_request.ik_seed_state.joint_state.name)
00386 buff.write(_struct_I.pack(length))
00387 for val1 in self.ik_request.ik_seed_state.joint_state.name:
00388 length = len(val1)
00389 buff.write(struct.pack('<I%ss'%length, length, val1))
00390 length = len(self.ik_request.ik_seed_state.joint_state.position)
00391 buff.write(_struct_I.pack(length))
00392 pattern = '<%sd'%length
00393 buff.write(struct.pack(pattern, *self.ik_request.ik_seed_state.joint_state.position))
00394 length = len(self.ik_request.ik_seed_state.joint_state.velocity)
00395 buff.write(_struct_I.pack(length))
00396 pattern = '<%sd'%length
00397 buff.write(struct.pack(pattern, *self.ik_request.ik_seed_state.joint_state.velocity))
00398 length = len(self.ik_request.ik_seed_state.joint_state.effort)
00399 buff.write(_struct_I.pack(length))
00400 pattern = '<%sd'%length
00401 buff.write(struct.pack(pattern, *self.ik_request.ik_seed_state.joint_state.effort))
00402 _x = self
00403 buff.write(_struct_2I.pack(_x.ik_request.ik_seed_state.multi_dof_joint_state.stamp.secs, _x.ik_request.ik_seed_state.multi_dof_joint_state.stamp.nsecs))
00404 length = len(self.ik_request.ik_seed_state.multi_dof_joint_state.joint_names)
00405 buff.write(_struct_I.pack(length))
00406 for val1 in self.ik_request.ik_seed_state.multi_dof_joint_state.joint_names:
00407 length = len(val1)
00408 buff.write(struct.pack('<I%ss'%length, length, val1))
00409 length = len(self.ik_request.ik_seed_state.multi_dof_joint_state.frame_ids)
00410 buff.write(_struct_I.pack(length))
00411 for val1 in self.ik_request.ik_seed_state.multi_dof_joint_state.frame_ids:
00412 length = len(val1)
00413 buff.write(struct.pack('<I%ss'%length, length, val1))
00414 length = len(self.ik_request.ik_seed_state.multi_dof_joint_state.child_frame_ids)
00415 buff.write(_struct_I.pack(length))
00416 for val1 in self.ik_request.ik_seed_state.multi_dof_joint_state.child_frame_ids:
00417 length = len(val1)
00418 buff.write(struct.pack('<I%ss'%length, length, val1))
00419 length = len(self.ik_request.ik_seed_state.multi_dof_joint_state.poses)
00420 buff.write(_struct_I.pack(length))
00421 for val1 in self.ik_request.ik_seed_state.multi_dof_joint_state.poses:
00422 _v1 = val1.position
00423 _x = _v1
00424 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00425 _v2 = val1.orientation
00426 _x = _v2
00427 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00428 _x = self
00429 buff.write(_struct_3I.pack(_x.ik_request.robot_state.joint_state.header.seq, _x.ik_request.robot_state.joint_state.header.stamp.secs, _x.ik_request.robot_state.joint_state.header.stamp.nsecs))
00430 _x = self.ik_request.robot_state.joint_state.header.frame_id
00431 length = len(_x)
00432 buff.write(struct.pack('<I%ss'%length, length, _x))
00433 length = len(self.ik_request.robot_state.joint_state.name)
00434 buff.write(_struct_I.pack(length))
00435 for val1 in self.ik_request.robot_state.joint_state.name:
00436 length = len(val1)
00437 buff.write(struct.pack('<I%ss'%length, length, val1))
00438 length = len(self.ik_request.robot_state.joint_state.position)
00439 buff.write(_struct_I.pack(length))
00440 pattern = '<%sd'%length
00441 buff.write(struct.pack(pattern, *self.ik_request.robot_state.joint_state.position))
00442 length = len(self.ik_request.robot_state.joint_state.velocity)
00443 buff.write(_struct_I.pack(length))
00444 pattern = '<%sd'%length
00445 buff.write(struct.pack(pattern, *self.ik_request.robot_state.joint_state.velocity))
00446 length = len(self.ik_request.robot_state.joint_state.effort)
00447 buff.write(_struct_I.pack(length))
00448 pattern = '<%sd'%length
00449 buff.write(struct.pack(pattern, *self.ik_request.robot_state.joint_state.effort))
00450 _x = self
00451 buff.write(_struct_2I.pack(_x.ik_request.robot_state.multi_dof_joint_state.stamp.secs, _x.ik_request.robot_state.multi_dof_joint_state.stamp.nsecs))
00452 length = len(self.ik_request.robot_state.multi_dof_joint_state.joint_names)
00453 buff.write(_struct_I.pack(length))
00454 for val1 in self.ik_request.robot_state.multi_dof_joint_state.joint_names:
00455 length = len(val1)
00456 buff.write(struct.pack('<I%ss'%length, length, val1))
00457 length = len(self.ik_request.robot_state.multi_dof_joint_state.frame_ids)
00458 buff.write(_struct_I.pack(length))
00459 for val1 in self.ik_request.robot_state.multi_dof_joint_state.frame_ids:
00460 length = len(val1)
00461 buff.write(struct.pack('<I%ss'%length, length, val1))
00462 length = len(self.ik_request.robot_state.multi_dof_joint_state.child_frame_ids)
00463 buff.write(_struct_I.pack(length))
00464 for val1 in self.ik_request.robot_state.multi_dof_joint_state.child_frame_ids:
00465 length = len(val1)
00466 buff.write(struct.pack('<I%ss'%length, length, val1))
00467 length = len(self.ik_request.robot_state.multi_dof_joint_state.poses)
00468 buff.write(_struct_I.pack(length))
00469 for val1 in self.ik_request.robot_state.multi_dof_joint_state.poses:
00470 _v3 = val1.position
00471 _x = _v3
00472 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00473 _v4 = val1.orientation
00474 _x = _v4
00475 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00476 length = len(self.allowed_contacts)
00477 buff.write(_struct_I.pack(length))
00478 for val1 in self.allowed_contacts:
00479 _x = val1.name
00480 length = len(_x)
00481 buff.write(struct.pack('<I%ss'%length, length, _x))
00482 _v5 = val1.shape
00483 buff.write(_struct_b.pack(_v5.type))
00484 length = len(_v5.dimensions)
00485 buff.write(_struct_I.pack(length))
00486 pattern = '<%sd'%length
00487 buff.write(struct.pack(pattern, *_v5.dimensions))
00488 length = len(_v5.triangles)
00489 buff.write(_struct_I.pack(length))
00490 pattern = '<%si'%length
00491 buff.write(struct.pack(pattern, *_v5.triangles))
00492 length = len(_v5.vertices)
00493 buff.write(_struct_I.pack(length))
00494 for val3 in _v5.vertices:
00495 _x = val3
00496 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00497 _v6 = val1.pose_stamped
00498 _v7 = _v6.header
00499 buff.write(_struct_I.pack(_v7.seq))
00500 _v8 = _v7.stamp
00501 _x = _v8
00502 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00503 _x = _v7.frame_id
00504 length = len(_x)
00505 buff.write(struct.pack('<I%ss'%length, length, _x))
00506 _v9 = _v6.pose
00507 _v10 = _v9.position
00508 _x = _v10
00509 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00510 _v11 = _v9.orientation
00511 _x = _v11
00512 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00513 length = len(val1.link_names)
00514 buff.write(_struct_I.pack(length))
00515 for val2 in val1.link_names:
00516 length = len(val2)
00517 buff.write(struct.pack('<I%ss'%length, length, val2))
00518 buff.write(_struct_d.pack(val1.penetration_depth))
00519 length = len(self.ordered_collision_operations.collision_operations)
00520 buff.write(_struct_I.pack(length))
00521 for val1 in self.ordered_collision_operations.collision_operations:
00522 _x = val1.object1
00523 length = len(_x)
00524 buff.write(struct.pack('<I%ss'%length, length, _x))
00525 _x = val1.object2
00526 length = len(_x)
00527 buff.write(struct.pack('<I%ss'%length, length, _x))
00528 _x = val1
00529 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
00530 length = len(self.link_padding)
00531 buff.write(_struct_I.pack(length))
00532 for val1 in self.link_padding:
00533 _x = val1.link_name
00534 length = len(_x)
00535 buff.write(struct.pack('<I%ss'%length, length, _x))
00536 buff.write(_struct_d.pack(val1.padding))
00537 length = len(self.constraints.joint_constraints)
00538 buff.write(_struct_I.pack(length))
00539 for val1 in self.constraints.joint_constraints:
00540 _x = val1.joint_name
00541 length = len(_x)
00542 buff.write(struct.pack('<I%ss'%length, length, _x))
00543 _x = val1
00544 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
00545 length = len(self.constraints.position_constraints)
00546 buff.write(_struct_I.pack(length))
00547 for val1 in self.constraints.position_constraints:
00548 _v12 = val1.header
00549 buff.write(_struct_I.pack(_v12.seq))
00550 _v13 = _v12.stamp
00551 _x = _v13
00552 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00553 _x = _v12.frame_id
00554 length = len(_x)
00555 buff.write(struct.pack('<I%ss'%length, length, _x))
00556 _x = val1.link_name
00557 length = len(_x)
00558 buff.write(struct.pack('<I%ss'%length, length, _x))
00559 _v14 = val1.target_point_offset
00560 _x = _v14
00561 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00562 _v15 = val1.position
00563 _x = _v15
00564 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00565 _v16 = val1.constraint_region_shape
00566 buff.write(_struct_b.pack(_v16.type))
00567 length = len(_v16.dimensions)
00568 buff.write(_struct_I.pack(length))
00569 pattern = '<%sd'%length
00570 buff.write(struct.pack(pattern, *_v16.dimensions))
00571 length = len(_v16.triangles)
00572 buff.write(_struct_I.pack(length))
00573 pattern = '<%si'%length
00574 buff.write(struct.pack(pattern, *_v16.triangles))
00575 length = len(_v16.vertices)
00576 buff.write(_struct_I.pack(length))
00577 for val3 in _v16.vertices:
00578 _x = val3
00579 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00580 _v17 = val1.constraint_region_orientation
00581 _x = _v17
00582 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00583 buff.write(_struct_d.pack(val1.weight))
00584 length = len(self.constraints.orientation_constraints)
00585 buff.write(_struct_I.pack(length))
00586 for val1 in self.constraints.orientation_constraints:
00587 _v18 = val1.header
00588 buff.write(_struct_I.pack(_v18.seq))
00589 _v19 = _v18.stamp
00590 _x = _v19
00591 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00592 _x = _v18.frame_id
00593 length = len(_x)
00594 buff.write(struct.pack('<I%ss'%length, length, _x))
00595 _x = val1.link_name
00596 length = len(_x)
00597 buff.write(struct.pack('<I%ss'%length, length, _x))
00598 buff.write(_struct_i.pack(val1.type))
00599 _v20 = val1.orientation
00600 _x = _v20
00601 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00602 _x = val1
00603 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
00604 length = len(self.constraints.visibility_constraints)
00605 buff.write(_struct_I.pack(length))
00606 for val1 in self.constraints.visibility_constraints:
00607 _v21 = val1.header
00608 buff.write(_struct_I.pack(_v21.seq))
00609 _v22 = _v21.stamp
00610 _x = _v22
00611 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00612 _x = _v21.frame_id
00613 length = len(_x)
00614 buff.write(struct.pack('<I%ss'%length, length, _x))
00615 _v23 = val1.target
00616 _v24 = _v23.header
00617 buff.write(_struct_I.pack(_v24.seq))
00618 _v25 = _v24.stamp
00619 _x = _v25
00620 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00621 _x = _v24.frame_id
00622 length = len(_x)
00623 buff.write(struct.pack('<I%ss'%length, length, _x))
00624 _v26 = _v23.point
00625 _x = _v26
00626 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00627 _v27 = val1.sensor_pose
00628 _v28 = _v27.header
00629 buff.write(_struct_I.pack(_v28.seq))
00630 _v29 = _v28.stamp
00631 _x = _v29
00632 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00633 _x = _v28.frame_id
00634 length = len(_x)
00635 buff.write(struct.pack('<I%ss'%length, length, _x))
00636 _v30 = _v27.pose
00637 _v31 = _v30.position
00638 _x = _v31
00639 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00640 _v32 = _v30.orientation
00641 _x = _v32
00642 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00643 buff.write(_struct_d.pack(val1.absolute_tolerance))
00644 _x = self
00645 buff.write(_struct_2i.pack(_x.timeout.secs, _x.timeout.nsecs))
00646 except struct.error, se: self._check_types(se)
00647 except TypeError, te: self._check_types(te)
00648
00649 def deserialize(self, str):
00650 """
00651 unpack serialized message in str into this message instance
00652 @param str: byte array of serialized message
00653 @type str: str
00654 """
00655 try:
00656 if self.ik_request is None:
00657 self.ik_request = kinematics_msgs.msg.PositionIKRequest()
00658 if self.ordered_collision_operations is None:
00659 self.ordered_collision_operations = motion_planning_msgs.msg.OrderedCollisionOperations()
00660 if self.constraints is None:
00661 self.constraints = motion_planning_msgs.msg.Constraints()
00662 if self.timeout is None:
00663 self.timeout = roslib.rostime.Duration()
00664 end = 0
00665 start = end
00666 end += 4
00667 (length,) = _struct_I.unpack(str[start:end])
00668 start = end
00669 end += length
00670 self.ik_request.ik_link_name = str[start:end]
00671 _x = self
00672 start = end
00673 end += 12
00674 (_x.ik_request.pose_stamped.header.seq, _x.ik_request.pose_stamped.header.stamp.secs, _x.ik_request.pose_stamped.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00675 start = end
00676 end += 4
00677 (length,) = _struct_I.unpack(str[start:end])
00678 start = end
00679 end += length
00680 self.ik_request.pose_stamped.header.frame_id = str[start:end]
00681 _x = self
00682 start = end
00683 end += 68
00684 (_x.ik_request.pose_stamped.pose.position.x, _x.ik_request.pose_stamped.pose.position.y, _x.ik_request.pose_stamped.pose.position.z, _x.ik_request.pose_stamped.pose.orientation.x, _x.ik_request.pose_stamped.pose.orientation.y, _x.ik_request.pose_stamped.pose.orientation.z, _x.ik_request.pose_stamped.pose.orientation.w, _x.ik_request.ik_seed_state.joint_state.header.seq, _x.ik_request.ik_seed_state.joint_state.header.stamp.secs, _x.ik_request.ik_seed_state.joint_state.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
00685 start = end
00686 end += 4
00687 (length,) = _struct_I.unpack(str[start:end])
00688 start = end
00689 end += length
00690 self.ik_request.ik_seed_state.joint_state.header.frame_id = str[start:end]
00691 start = end
00692 end += 4
00693 (length,) = _struct_I.unpack(str[start:end])
00694 self.ik_request.ik_seed_state.joint_state.name = []
00695 for i in xrange(0, length):
00696 start = end
00697 end += 4
00698 (length,) = _struct_I.unpack(str[start:end])
00699 start = end
00700 end += length
00701 val1 = str[start:end]
00702 self.ik_request.ik_seed_state.joint_state.name.append(val1)
00703 start = end
00704 end += 4
00705 (length,) = _struct_I.unpack(str[start:end])
00706 pattern = '<%sd'%length
00707 start = end
00708 end += struct.calcsize(pattern)
00709 self.ik_request.ik_seed_state.joint_state.position = struct.unpack(pattern, str[start:end])
00710 start = end
00711 end += 4
00712 (length,) = _struct_I.unpack(str[start:end])
00713 pattern = '<%sd'%length
00714 start = end
00715 end += struct.calcsize(pattern)
00716 self.ik_request.ik_seed_state.joint_state.velocity = struct.unpack(pattern, str[start:end])
00717 start = end
00718 end += 4
00719 (length,) = _struct_I.unpack(str[start:end])
00720 pattern = '<%sd'%length
00721 start = end
00722 end += struct.calcsize(pattern)
00723 self.ik_request.ik_seed_state.joint_state.effort = struct.unpack(pattern, str[start:end])
00724 _x = self
00725 start = end
00726 end += 8
00727 (_x.ik_request.ik_seed_state.multi_dof_joint_state.stamp.secs, _x.ik_request.ik_seed_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00728 start = end
00729 end += 4
00730 (length,) = _struct_I.unpack(str[start:end])
00731 self.ik_request.ik_seed_state.multi_dof_joint_state.joint_names = []
00732 for i in xrange(0, length):
00733 start = end
00734 end += 4
00735 (length,) = _struct_I.unpack(str[start:end])
00736 start = end
00737 end += length
00738 val1 = str[start:end]
00739 self.ik_request.ik_seed_state.multi_dof_joint_state.joint_names.append(val1)
00740 start = end
00741 end += 4
00742 (length,) = _struct_I.unpack(str[start:end])
00743 self.ik_request.ik_seed_state.multi_dof_joint_state.frame_ids = []
00744 for i in xrange(0, length):
00745 start = end
00746 end += 4
00747 (length,) = _struct_I.unpack(str[start:end])
00748 start = end
00749 end += length
00750 val1 = str[start:end]
00751 self.ik_request.ik_seed_state.multi_dof_joint_state.frame_ids.append(val1)
00752 start = end
00753 end += 4
00754 (length,) = _struct_I.unpack(str[start:end])
00755 self.ik_request.ik_seed_state.multi_dof_joint_state.child_frame_ids = []
00756 for i in xrange(0, length):
00757 start = end
00758 end += 4
00759 (length,) = _struct_I.unpack(str[start:end])
00760 start = end
00761 end += length
00762 val1 = str[start:end]
00763 self.ik_request.ik_seed_state.multi_dof_joint_state.child_frame_ids.append(val1)
00764 start = end
00765 end += 4
00766 (length,) = _struct_I.unpack(str[start:end])
00767 self.ik_request.ik_seed_state.multi_dof_joint_state.poses = []
00768 for i in xrange(0, length):
00769 val1 = geometry_msgs.msg.Pose()
00770 _v33 = val1.position
00771 _x = _v33
00772 start = end
00773 end += 24
00774 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00775 _v34 = val1.orientation
00776 _x = _v34
00777 start = end
00778 end += 32
00779 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00780 self.ik_request.ik_seed_state.multi_dof_joint_state.poses.append(val1)
00781 _x = self
00782 start = end
00783 end += 12
00784 (_x.ik_request.robot_state.joint_state.header.seq, _x.ik_request.robot_state.joint_state.header.stamp.secs, _x.ik_request.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00785 start = end
00786 end += 4
00787 (length,) = _struct_I.unpack(str[start:end])
00788 start = end
00789 end += length
00790 self.ik_request.robot_state.joint_state.header.frame_id = str[start:end]
00791 start = end
00792 end += 4
00793 (length,) = _struct_I.unpack(str[start:end])
00794 self.ik_request.robot_state.joint_state.name = []
00795 for i in xrange(0, length):
00796 start = end
00797 end += 4
00798 (length,) = _struct_I.unpack(str[start:end])
00799 start = end
00800 end += length
00801 val1 = str[start:end]
00802 self.ik_request.robot_state.joint_state.name.append(val1)
00803 start = end
00804 end += 4
00805 (length,) = _struct_I.unpack(str[start:end])
00806 pattern = '<%sd'%length
00807 start = end
00808 end += struct.calcsize(pattern)
00809 self.ik_request.robot_state.joint_state.position = struct.unpack(pattern, str[start:end])
00810 start = end
00811 end += 4
00812 (length,) = _struct_I.unpack(str[start:end])
00813 pattern = '<%sd'%length
00814 start = end
00815 end += struct.calcsize(pattern)
00816 self.ik_request.robot_state.joint_state.velocity = struct.unpack(pattern, str[start:end])
00817 start = end
00818 end += 4
00819 (length,) = _struct_I.unpack(str[start:end])
00820 pattern = '<%sd'%length
00821 start = end
00822 end += struct.calcsize(pattern)
00823 self.ik_request.robot_state.joint_state.effort = struct.unpack(pattern, str[start:end])
00824 _x = self
00825 start = end
00826 end += 8
00827 (_x.ik_request.robot_state.multi_dof_joint_state.stamp.secs, _x.ik_request.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00828 start = end
00829 end += 4
00830 (length,) = _struct_I.unpack(str[start:end])
00831 self.ik_request.robot_state.multi_dof_joint_state.joint_names = []
00832 for i in xrange(0, length):
00833 start = end
00834 end += 4
00835 (length,) = _struct_I.unpack(str[start:end])
00836 start = end
00837 end += length
00838 val1 = str[start:end]
00839 self.ik_request.robot_state.multi_dof_joint_state.joint_names.append(val1)
00840 start = end
00841 end += 4
00842 (length,) = _struct_I.unpack(str[start:end])
00843 self.ik_request.robot_state.multi_dof_joint_state.frame_ids = []
00844 for i in xrange(0, length):
00845 start = end
00846 end += 4
00847 (length,) = _struct_I.unpack(str[start:end])
00848 start = end
00849 end += length
00850 val1 = str[start:end]
00851 self.ik_request.robot_state.multi_dof_joint_state.frame_ids.append(val1)
00852 start = end
00853 end += 4
00854 (length,) = _struct_I.unpack(str[start:end])
00855 self.ik_request.robot_state.multi_dof_joint_state.child_frame_ids = []
00856 for i in xrange(0, length):
00857 start = end
00858 end += 4
00859 (length,) = _struct_I.unpack(str[start:end])
00860 start = end
00861 end += length
00862 val1 = str[start:end]
00863 self.ik_request.robot_state.multi_dof_joint_state.child_frame_ids.append(val1)
00864 start = end
00865 end += 4
00866 (length,) = _struct_I.unpack(str[start:end])
00867 self.ik_request.robot_state.multi_dof_joint_state.poses = []
00868 for i in xrange(0, length):
00869 val1 = geometry_msgs.msg.Pose()
00870 _v35 = val1.position
00871 _x = _v35
00872 start = end
00873 end += 24
00874 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00875 _v36 = val1.orientation
00876 _x = _v36
00877 start = end
00878 end += 32
00879 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00880 self.ik_request.robot_state.multi_dof_joint_state.poses.append(val1)
00881 start = end
00882 end += 4
00883 (length,) = _struct_I.unpack(str[start:end])
00884 self.allowed_contacts = []
00885 for i in xrange(0, length):
00886 val1 = motion_planning_msgs.msg.AllowedContactSpecification()
00887 start = end
00888 end += 4
00889 (length,) = _struct_I.unpack(str[start:end])
00890 start = end
00891 end += length
00892 val1.name = str[start:end]
00893 _v37 = val1.shape
00894 start = end
00895 end += 1
00896 (_v37.type,) = _struct_b.unpack(str[start:end])
00897 start = end
00898 end += 4
00899 (length,) = _struct_I.unpack(str[start:end])
00900 pattern = '<%sd'%length
00901 start = end
00902 end += struct.calcsize(pattern)
00903 _v37.dimensions = struct.unpack(pattern, str[start:end])
00904 start = end
00905 end += 4
00906 (length,) = _struct_I.unpack(str[start:end])
00907 pattern = '<%si'%length
00908 start = end
00909 end += struct.calcsize(pattern)
00910 _v37.triangles = struct.unpack(pattern, str[start:end])
00911 start = end
00912 end += 4
00913 (length,) = _struct_I.unpack(str[start:end])
00914 _v37.vertices = []
00915 for i in xrange(0, length):
00916 val3 = geometry_msgs.msg.Point()
00917 _x = val3
00918 start = end
00919 end += 24
00920 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00921 _v37.vertices.append(val3)
00922 _v38 = val1.pose_stamped
00923 _v39 = _v38.header
00924 start = end
00925 end += 4
00926 (_v39.seq,) = _struct_I.unpack(str[start:end])
00927 _v40 = _v39.stamp
00928 _x = _v40
00929 start = end
00930 end += 8
00931 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00932 start = end
00933 end += 4
00934 (length,) = _struct_I.unpack(str[start:end])
00935 start = end
00936 end += length
00937 _v39.frame_id = str[start:end]
00938 _v41 = _v38.pose
00939 _v42 = _v41.position
00940 _x = _v42
00941 start = end
00942 end += 24
00943 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00944 _v43 = _v41.orientation
00945 _x = _v43
00946 start = end
00947 end += 32
00948 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00949 start = end
00950 end += 4
00951 (length,) = _struct_I.unpack(str[start:end])
00952 val1.link_names = []
00953 for i in xrange(0, length):
00954 start = end
00955 end += 4
00956 (length,) = _struct_I.unpack(str[start:end])
00957 start = end
00958 end += length
00959 val2 = str[start:end]
00960 val1.link_names.append(val2)
00961 start = end
00962 end += 8
00963 (val1.penetration_depth,) = _struct_d.unpack(str[start:end])
00964 self.allowed_contacts.append(val1)
00965 start = end
00966 end += 4
00967 (length,) = _struct_I.unpack(str[start:end])
00968 self.ordered_collision_operations.collision_operations = []
00969 for i in xrange(0, length):
00970 val1 = motion_planning_msgs.msg.CollisionOperation()
00971 start = end
00972 end += 4
00973 (length,) = _struct_I.unpack(str[start:end])
00974 start = end
00975 end += length
00976 val1.object1 = str[start:end]
00977 start = end
00978 end += 4
00979 (length,) = _struct_I.unpack(str[start:end])
00980 start = end
00981 end += length
00982 val1.object2 = str[start:end]
00983 _x = val1
00984 start = end
00985 end += 12
00986 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
00987 self.ordered_collision_operations.collision_operations.append(val1)
00988 start = end
00989 end += 4
00990 (length,) = _struct_I.unpack(str[start:end])
00991 self.link_padding = []
00992 for i in xrange(0, length):
00993 val1 = motion_planning_msgs.msg.LinkPadding()
00994 start = end
00995 end += 4
00996 (length,) = _struct_I.unpack(str[start:end])
00997 start = end
00998 end += length
00999 val1.link_name = str[start:end]
01000 start = end
01001 end += 8
01002 (val1.padding,) = _struct_d.unpack(str[start:end])
01003 self.link_padding.append(val1)
01004 start = end
01005 end += 4
01006 (length,) = _struct_I.unpack(str[start:end])
01007 self.constraints.joint_constraints = []
01008 for i in xrange(0, length):
01009 val1 = motion_planning_msgs.msg.JointConstraint()
01010 start = end
01011 end += 4
01012 (length,) = _struct_I.unpack(str[start:end])
01013 start = end
01014 end += length
01015 val1.joint_name = str[start:end]
01016 _x = val1
01017 start = end
01018 end += 32
01019 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
01020 self.constraints.joint_constraints.append(val1)
01021 start = end
01022 end += 4
01023 (length,) = _struct_I.unpack(str[start:end])
01024 self.constraints.position_constraints = []
01025 for i in xrange(0, length):
01026 val1 = motion_planning_msgs.msg.PositionConstraint()
01027 _v44 = val1.header
01028 start = end
01029 end += 4
01030 (_v44.seq,) = _struct_I.unpack(str[start:end])
01031 _v45 = _v44.stamp
01032 _x = _v45
01033 start = end
01034 end += 8
01035 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01036 start = end
01037 end += 4
01038 (length,) = _struct_I.unpack(str[start:end])
01039 start = end
01040 end += length
01041 _v44.frame_id = str[start:end]
01042 start = end
01043 end += 4
01044 (length,) = _struct_I.unpack(str[start:end])
01045 start = end
01046 end += length
01047 val1.link_name = str[start:end]
01048 _v46 = val1.target_point_offset
01049 _x = _v46
01050 start = end
01051 end += 24
01052 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01053 _v47 = val1.position
01054 _x = _v47
01055 start = end
01056 end += 24
01057 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01058 _v48 = val1.constraint_region_shape
01059 start = end
01060 end += 1
01061 (_v48.type,) = _struct_b.unpack(str[start:end])
01062 start = end
01063 end += 4
01064 (length,) = _struct_I.unpack(str[start:end])
01065 pattern = '<%sd'%length
01066 start = end
01067 end += struct.calcsize(pattern)
01068 _v48.dimensions = struct.unpack(pattern, str[start:end])
01069 start = end
01070 end += 4
01071 (length,) = _struct_I.unpack(str[start:end])
01072 pattern = '<%si'%length
01073 start = end
01074 end += struct.calcsize(pattern)
01075 _v48.triangles = struct.unpack(pattern, str[start:end])
01076 start = end
01077 end += 4
01078 (length,) = _struct_I.unpack(str[start:end])
01079 _v48.vertices = []
01080 for i in xrange(0, length):
01081 val3 = geometry_msgs.msg.Point()
01082 _x = val3
01083 start = end
01084 end += 24
01085 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01086 _v48.vertices.append(val3)
01087 _v49 = val1.constraint_region_orientation
01088 _x = _v49
01089 start = end
01090 end += 32
01091 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01092 start = end
01093 end += 8
01094 (val1.weight,) = _struct_d.unpack(str[start:end])
01095 self.constraints.position_constraints.append(val1)
01096 start = end
01097 end += 4
01098 (length,) = _struct_I.unpack(str[start:end])
01099 self.constraints.orientation_constraints = []
01100 for i in xrange(0, length):
01101 val1 = motion_planning_msgs.msg.OrientationConstraint()
01102 _v50 = val1.header
01103 start = end
01104 end += 4
01105 (_v50.seq,) = _struct_I.unpack(str[start:end])
01106 _v51 = _v50.stamp
01107 _x = _v51
01108 start = end
01109 end += 8
01110 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01111 start = end
01112 end += 4
01113 (length,) = _struct_I.unpack(str[start:end])
01114 start = end
01115 end += length
01116 _v50.frame_id = str[start:end]
01117 start = end
01118 end += 4
01119 (length,) = _struct_I.unpack(str[start:end])
01120 start = end
01121 end += length
01122 val1.link_name = str[start:end]
01123 start = end
01124 end += 4
01125 (val1.type,) = _struct_i.unpack(str[start:end])
01126 _v52 = val1.orientation
01127 _x = _v52
01128 start = end
01129 end += 32
01130 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01131 _x = val1
01132 start = end
01133 end += 32
01134 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
01135 self.constraints.orientation_constraints.append(val1)
01136 start = end
01137 end += 4
01138 (length,) = _struct_I.unpack(str[start:end])
01139 self.constraints.visibility_constraints = []
01140 for i in xrange(0, length):
01141 val1 = motion_planning_msgs.msg.VisibilityConstraint()
01142 _v53 = val1.header
01143 start = end
01144 end += 4
01145 (_v53.seq,) = _struct_I.unpack(str[start:end])
01146 _v54 = _v53.stamp
01147 _x = _v54
01148 start = end
01149 end += 8
01150 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01151 start = end
01152 end += 4
01153 (length,) = _struct_I.unpack(str[start:end])
01154 start = end
01155 end += length
01156 _v53.frame_id = str[start:end]
01157 _v55 = val1.target
01158 _v56 = _v55.header
01159 start = end
01160 end += 4
01161 (_v56.seq,) = _struct_I.unpack(str[start:end])
01162 _v57 = _v56.stamp
01163 _x = _v57
01164 start = end
01165 end += 8
01166 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01167 start = end
01168 end += 4
01169 (length,) = _struct_I.unpack(str[start:end])
01170 start = end
01171 end += length
01172 _v56.frame_id = str[start:end]
01173 _v58 = _v55.point
01174 _x = _v58
01175 start = end
01176 end += 24
01177 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01178 _v59 = val1.sensor_pose
01179 _v60 = _v59.header
01180 start = end
01181 end += 4
01182 (_v60.seq,) = _struct_I.unpack(str[start:end])
01183 _v61 = _v60.stamp
01184 _x = _v61
01185 start = end
01186 end += 8
01187 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01188 start = end
01189 end += 4
01190 (length,) = _struct_I.unpack(str[start:end])
01191 start = end
01192 end += length
01193 _v60.frame_id = str[start:end]
01194 _v62 = _v59.pose
01195 _v63 = _v62.position
01196 _x = _v63
01197 start = end
01198 end += 24
01199 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01200 _v64 = _v62.orientation
01201 _x = _v64
01202 start = end
01203 end += 32
01204 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01205 start = end
01206 end += 8
01207 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
01208 self.constraints.visibility_constraints.append(val1)
01209 _x = self
01210 start = end
01211 end += 8
01212 (_x.timeout.secs, _x.timeout.nsecs,) = _struct_2i.unpack(str[start:end])
01213 self.timeout.canon()
01214 return self
01215 except struct.error, e:
01216 raise roslib.message.DeserializationError(e)
01217
01218
01219 def serialize_numpy(self, buff, numpy):
01220 """
01221 serialize message with numpy array types into buffer
01222 @param buff: buffer
01223 @type buff: StringIO
01224 @param numpy: numpy python module
01225 @type numpy module
01226 """
01227 try:
01228 _x = self.ik_request.ik_link_name
01229 length = len(_x)
01230 buff.write(struct.pack('<I%ss'%length, length, _x))
01231 _x = self
01232 buff.write(_struct_3I.pack(_x.ik_request.pose_stamped.header.seq, _x.ik_request.pose_stamped.header.stamp.secs, _x.ik_request.pose_stamped.header.stamp.nsecs))
01233 _x = self.ik_request.pose_stamped.header.frame_id
01234 length = len(_x)
01235 buff.write(struct.pack('<I%ss'%length, length, _x))
01236 _x = self
01237 buff.write(_struct_7d3I.pack(_x.ik_request.pose_stamped.pose.position.x, _x.ik_request.pose_stamped.pose.position.y, _x.ik_request.pose_stamped.pose.position.z, _x.ik_request.pose_stamped.pose.orientation.x, _x.ik_request.pose_stamped.pose.orientation.y, _x.ik_request.pose_stamped.pose.orientation.z, _x.ik_request.pose_stamped.pose.orientation.w, _x.ik_request.ik_seed_state.joint_state.header.seq, _x.ik_request.ik_seed_state.joint_state.header.stamp.secs, _x.ik_request.ik_seed_state.joint_state.header.stamp.nsecs))
01238 _x = self.ik_request.ik_seed_state.joint_state.header.frame_id
01239 length = len(_x)
01240 buff.write(struct.pack('<I%ss'%length, length, _x))
01241 length = len(self.ik_request.ik_seed_state.joint_state.name)
01242 buff.write(_struct_I.pack(length))
01243 for val1 in self.ik_request.ik_seed_state.joint_state.name:
01244 length = len(val1)
01245 buff.write(struct.pack('<I%ss'%length, length, val1))
01246 length = len(self.ik_request.ik_seed_state.joint_state.position)
01247 buff.write(_struct_I.pack(length))
01248 pattern = '<%sd'%length
01249 buff.write(self.ik_request.ik_seed_state.joint_state.position.tostring())
01250 length = len(self.ik_request.ik_seed_state.joint_state.velocity)
01251 buff.write(_struct_I.pack(length))
01252 pattern = '<%sd'%length
01253 buff.write(self.ik_request.ik_seed_state.joint_state.velocity.tostring())
01254 length = len(self.ik_request.ik_seed_state.joint_state.effort)
01255 buff.write(_struct_I.pack(length))
01256 pattern = '<%sd'%length
01257 buff.write(self.ik_request.ik_seed_state.joint_state.effort.tostring())
01258 _x = self
01259 buff.write(_struct_2I.pack(_x.ik_request.ik_seed_state.multi_dof_joint_state.stamp.secs, _x.ik_request.ik_seed_state.multi_dof_joint_state.stamp.nsecs))
01260 length = len(self.ik_request.ik_seed_state.multi_dof_joint_state.joint_names)
01261 buff.write(_struct_I.pack(length))
01262 for val1 in self.ik_request.ik_seed_state.multi_dof_joint_state.joint_names:
01263 length = len(val1)
01264 buff.write(struct.pack('<I%ss'%length, length, val1))
01265 length = len(self.ik_request.ik_seed_state.multi_dof_joint_state.frame_ids)
01266 buff.write(_struct_I.pack(length))
01267 for val1 in self.ik_request.ik_seed_state.multi_dof_joint_state.frame_ids:
01268 length = len(val1)
01269 buff.write(struct.pack('<I%ss'%length, length, val1))
01270 length = len(self.ik_request.ik_seed_state.multi_dof_joint_state.child_frame_ids)
01271 buff.write(_struct_I.pack(length))
01272 for val1 in self.ik_request.ik_seed_state.multi_dof_joint_state.child_frame_ids:
01273 length = len(val1)
01274 buff.write(struct.pack('<I%ss'%length, length, val1))
01275 length = len(self.ik_request.ik_seed_state.multi_dof_joint_state.poses)
01276 buff.write(_struct_I.pack(length))
01277 for val1 in self.ik_request.ik_seed_state.multi_dof_joint_state.poses:
01278 _v65 = val1.position
01279 _x = _v65
01280 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01281 _v66 = val1.orientation
01282 _x = _v66
01283 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01284 _x = self
01285 buff.write(_struct_3I.pack(_x.ik_request.robot_state.joint_state.header.seq, _x.ik_request.robot_state.joint_state.header.stamp.secs, _x.ik_request.robot_state.joint_state.header.stamp.nsecs))
01286 _x = self.ik_request.robot_state.joint_state.header.frame_id
01287 length = len(_x)
01288 buff.write(struct.pack('<I%ss'%length, length, _x))
01289 length = len(self.ik_request.robot_state.joint_state.name)
01290 buff.write(_struct_I.pack(length))
01291 for val1 in self.ik_request.robot_state.joint_state.name:
01292 length = len(val1)
01293 buff.write(struct.pack('<I%ss'%length, length, val1))
01294 length = len(self.ik_request.robot_state.joint_state.position)
01295 buff.write(_struct_I.pack(length))
01296 pattern = '<%sd'%length
01297 buff.write(self.ik_request.robot_state.joint_state.position.tostring())
01298 length = len(self.ik_request.robot_state.joint_state.velocity)
01299 buff.write(_struct_I.pack(length))
01300 pattern = '<%sd'%length
01301 buff.write(self.ik_request.robot_state.joint_state.velocity.tostring())
01302 length = len(self.ik_request.robot_state.joint_state.effort)
01303 buff.write(_struct_I.pack(length))
01304 pattern = '<%sd'%length
01305 buff.write(self.ik_request.robot_state.joint_state.effort.tostring())
01306 _x = self
01307 buff.write(_struct_2I.pack(_x.ik_request.robot_state.multi_dof_joint_state.stamp.secs, _x.ik_request.robot_state.multi_dof_joint_state.stamp.nsecs))
01308 length = len(self.ik_request.robot_state.multi_dof_joint_state.joint_names)
01309 buff.write(_struct_I.pack(length))
01310 for val1 in self.ik_request.robot_state.multi_dof_joint_state.joint_names:
01311 length = len(val1)
01312 buff.write(struct.pack('<I%ss'%length, length, val1))
01313 length = len(self.ik_request.robot_state.multi_dof_joint_state.frame_ids)
01314 buff.write(_struct_I.pack(length))
01315 for val1 in self.ik_request.robot_state.multi_dof_joint_state.frame_ids:
01316 length = len(val1)
01317 buff.write(struct.pack('<I%ss'%length, length, val1))
01318 length = len(self.ik_request.robot_state.multi_dof_joint_state.child_frame_ids)
01319 buff.write(_struct_I.pack(length))
01320 for val1 in self.ik_request.robot_state.multi_dof_joint_state.child_frame_ids:
01321 length = len(val1)
01322 buff.write(struct.pack('<I%ss'%length, length, val1))
01323 length = len(self.ik_request.robot_state.multi_dof_joint_state.poses)
01324 buff.write(_struct_I.pack(length))
01325 for val1 in self.ik_request.robot_state.multi_dof_joint_state.poses:
01326 _v67 = val1.position
01327 _x = _v67
01328 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01329 _v68 = val1.orientation
01330 _x = _v68
01331 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01332 length = len(self.allowed_contacts)
01333 buff.write(_struct_I.pack(length))
01334 for val1 in self.allowed_contacts:
01335 _x = val1.name
01336 length = len(_x)
01337 buff.write(struct.pack('<I%ss'%length, length, _x))
01338 _v69 = val1.shape
01339 buff.write(_struct_b.pack(_v69.type))
01340 length = len(_v69.dimensions)
01341 buff.write(_struct_I.pack(length))
01342 pattern = '<%sd'%length
01343 buff.write(_v69.dimensions.tostring())
01344 length = len(_v69.triangles)
01345 buff.write(_struct_I.pack(length))
01346 pattern = '<%si'%length
01347 buff.write(_v69.triangles.tostring())
01348 length = len(_v69.vertices)
01349 buff.write(_struct_I.pack(length))
01350 for val3 in _v69.vertices:
01351 _x = val3
01352 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01353 _v70 = val1.pose_stamped
01354 _v71 = _v70.header
01355 buff.write(_struct_I.pack(_v71.seq))
01356 _v72 = _v71.stamp
01357 _x = _v72
01358 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01359 _x = _v71.frame_id
01360 length = len(_x)
01361 buff.write(struct.pack('<I%ss'%length, length, _x))
01362 _v73 = _v70.pose
01363 _v74 = _v73.position
01364 _x = _v74
01365 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01366 _v75 = _v73.orientation
01367 _x = _v75
01368 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01369 length = len(val1.link_names)
01370 buff.write(_struct_I.pack(length))
01371 for val2 in val1.link_names:
01372 length = len(val2)
01373 buff.write(struct.pack('<I%ss'%length, length, val2))
01374 buff.write(_struct_d.pack(val1.penetration_depth))
01375 length = len(self.ordered_collision_operations.collision_operations)
01376 buff.write(_struct_I.pack(length))
01377 for val1 in self.ordered_collision_operations.collision_operations:
01378 _x = val1.object1
01379 length = len(_x)
01380 buff.write(struct.pack('<I%ss'%length, length, _x))
01381 _x = val1.object2
01382 length = len(_x)
01383 buff.write(struct.pack('<I%ss'%length, length, _x))
01384 _x = val1
01385 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
01386 length = len(self.link_padding)
01387 buff.write(_struct_I.pack(length))
01388 for val1 in self.link_padding:
01389 _x = val1.link_name
01390 length = len(_x)
01391 buff.write(struct.pack('<I%ss'%length, length, _x))
01392 buff.write(_struct_d.pack(val1.padding))
01393 length = len(self.constraints.joint_constraints)
01394 buff.write(_struct_I.pack(length))
01395 for val1 in self.constraints.joint_constraints:
01396 _x = val1.joint_name
01397 length = len(_x)
01398 buff.write(struct.pack('<I%ss'%length, length, _x))
01399 _x = val1
01400 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
01401 length = len(self.constraints.position_constraints)
01402 buff.write(_struct_I.pack(length))
01403 for val1 in self.constraints.position_constraints:
01404 _v76 = val1.header
01405 buff.write(_struct_I.pack(_v76.seq))
01406 _v77 = _v76.stamp
01407 _x = _v77
01408 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01409 _x = _v76.frame_id
01410 length = len(_x)
01411 buff.write(struct.pack('<I%ss'%length, length, _x))
01412 _x = val1.link_name
01413 length = len(_x)
01414 buff.write(struct.pack('<I%ss'%length, length, _x))
01415 _v78 = val1.target_point_offset
01416 _x = _v78
01417 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01418 _v79 = val1.position
01419 _x = _v79
01420 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01421 _v80 = val1.constraint_region_shape
01422 buff.write(_struct_b.pack(_v80.type))
01423 length = len(_v80.dimensions)
01424 buff.write(_struct_I.pack(length))
01425 pattern = '<%sd'%length
01426 buff.write(_v80.dimensions.tostring())
01427 length = len(_v80.triangles)
01428 buff.write(_struct_I.pack(length))
01429 pattern = '<%si'%length
01430 buff.write(_v80.triangles.tostring())
01431 length = len(_v80.vertices)
01432 buff.write(_struct_I.pack(length))
01433 for val3 in _v80.vertices:
01434 _x = val3
01435 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01436 _v81 = val1.constraint_region_orientation
01437 _x = _v81
01438 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01439 buff.write(_struct_d.pack(val1.weight))
01440 length = len(self.constraints.orientation_constraints)
01441 buff.write(_struct_I.pack(length))
01442 for val1 in self.constraints.orientation_constraints:
01443 _v82 = val1.header
01444 buff.write(_struct_I.pack(_v82.seq))
01445 _v83 = _v82.stamp
01446 _x = _v83
01447 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01448 _x = _v82.frame_id
01449 length = len(_x)
01450 buff.write(struct.pack('<I%ss'%length, length, _x))
01451 _x = val1.link_name
01452 length = len(_x)
01453 buff.write(struct.pack('<I%ss'%length, length, _x))
01454 buff.write(_struct_i.pack(val1.type))
01455 _v84 = val1.orientation
01456 _x = _v84
01457 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01458 _x = val1
01459 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
01460 length = len(self.constraints.visibility_constraints)
01461 buff.write(_struct_I.pack(length))
01462 for val1 in self.constraints.visibility_constraints:
01463 _v85 = val1.header
01464 buff.write(_struct_I.pack(_v85.seq))
01465 _v86 = _v85.stamp
01466 _x = _v86
01467 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01468 _x = _v85.frame_id
01469 length = len(_x)
01470 buff.write(struct.pack('<I%ss'%length, length, _x))
01471 _v87 = val1.target
01472 _v88 = _v87.header
01473 buff.write(_struct_I.pack(_v88.seq))
01474 _v89 = _v88.stamp
01475 _x = _v89
01476 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01477 _x = _v88.frame_id
01478 length = len(_x)
01479 buff.write(struct.pack('<I%ss'%length, length, _x))
01480 _v90 = _v87.point
01481 _x = _v90
01482 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01483 _v91 = val1.sensor_pose
01484 _v92 = _v91.header
01485 buff.write(_struct_I.pack(_v92.seq))
01486 _v93 = _v92.stamp
01487 _x = _v93
01488 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01489 _x = _v92.frame_id
01490 length = len(_x)
01491 buff.write(struct.pack('<I%ss'%length, length, _x))
01492 _v94 = _v91.pose
01493 _v95 = _v94.position
01494 _x = _v95
01495 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01496 _v96 = _v94.orientation
01497 _x = _v96
01498 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01499 buff.write(_struct_d.pack(val1.absolute_tolerance))
01500 _x = self
01501 buff.write(_struct_2i.pack(_x.timeout.secs, _x.timeout.nsecs))
01502 except struct.error, se: self._check_types(se)
01503 except TypeError, te: self._check_types(te)
01504
01505 def deserialize_numpy(self, str, numpy):
01506 """
01507 unpack serialized message in str into this message instance using numpy for array types
01508 @param str: byte array of serialized message
01509 @type str: str
01510 @param numpy: numpy python module
01511 @type numpy: module
01512 """
01513 try:
01514 if self.ik_request is None:
01515 self.ik_request = kinematics_msgs.msg.PositionIKRequest()
01516 if self.ordered_collision_operations is None:
01517 self.ordered_collision_operations = motion_planning_msgs.msg.OrderedCollisionOperations()
01518 if self.constraints is None:
01519 self.constraints = motion_planning_msgs.msg.Constraints()
01520 if self.timeout is None:
01521 self.timeout = roslib.rostime.Duration()
01522 end = 0
01523 start = end
01524 end += 4
01525 (length,) = _struct_I.unpack(str[start:end])
01526 start = end
01527 end += length
01528 self.ik_request.ik_link_name = str[start:end]
01529 _x = self
01530 start = end
01531 end += 12
01532 (_x.ik_request.pose_stamped.header.seq, _x.ik_request.pose_stamped.header.stamp.secs, _x.ik_request.pose_stamped.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01533 start = end
01534 end += 4
01535 (length,) = _struct_I.unpack(str[start:end])
01536 start = end
01537 end += length
01538 self.ik_request.pose_stamped.header.frame_id = str[start:end]
01539 _x = self
01540 start = end
01541 end += 68
01542 (_x.ik_request.pose_stamped.pose.position.x, _x.ik_request.pose_stamped.pose.position.y, _x.ik_request.pose_stamped.pose.position.z, _x.ik_request.pose_stamped.pose.orientation.x, _x.ik_request.pose_stamped.pose.orientation.y, _x.ik_request.pose_stamped.pose.orientation.z, _x.ik_request.pose_stamped.pose.orientation.w, _x.ik_request.ik_seed_state.joint_state.header.seq, _x.ik_request.ik_seed_state.joint_state.header.stamp.secs, _x.ik_request.ik_seed_state.joint_state.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
01543 start = end
01544 end += 4
01545 (length,) = _struct_I.unpack(str[start:end])
01546 start = end
01547 end += length
01548 self.ik_request.ik_seed_state.joint_state.header.frame_id = str[start:end]
01549 start = end
01550 end += 4
01551 (length,) = _struct_I.unpack(str[start:end])
01552 self.ik_request.ik_seed_state.joint_state.name = []
01553 for i in xrange(0, length):
01554 start = end
01555 end += 4
01556 (length,) = _struct_I.unpack(str[start:end])
01557 start = end
01558 end += length
01559 val1 = str[start:end]
01560 self.ik_request.ik_seed_state.joint_state.name.append(val1)
01561 start = end
01562 end += 4
01563 (length,) = _struct_I.unpack(str[start:end])
01564 pattern = '<%sd'%length
01565 start = end
01566 end += struct.calcsize(pattern)
01567 self.ik_request.ik_seed_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01568 start = end
01569 end += 4
01570 (length,) = _struct_I.unpack(str[start:end])
01571 pattern = '<%sd'%length
01572 start = end
01573 end += struct.calcsize(pattern)
01574 self.ik_request.ik_seed_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01575 start = end
01576 end += 4
01577 (length,) = _struct_I.unpack(str[start:end])
01578 pattern = '<%sd'%length
01579 start = end
01580 end += struct.calcsize(pattern)
01581 self.ik_request.ik_seed_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01582 _x = self
01583 start = end
01584 end += 8
01585 (_x.ik_request.ik_seed_state.multi_dof_joint_state.stamp.secs, _x.ik_request.ik_seed_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01586 start = end
01587 end += 4
01588 (length,) = _struct_I.unpack(str[start:end])
01589 self.ik_request.ik_seed_state.multi_dof_joint_state.joint_names = []
01590 for i in xrange(0, length):
01591 start = end
01592 end += 4
01593 (length,) = _struct_I.unpack(str[start:end])
01594 start = end
01595 end += length
01596 val1 = str[start:end]
01597 self.ik_request.ik_seed_state.multi_dof_joint_state.joint_names.append(val1)
01598 start = end
01599 end += 4
01600 (length,) = _struct_I.unpack(str[start:end])
01601 self.ik_request.ik_seed_state.multi_dof_joint_state.frame_ids = []
01602 for i in xrange(0, length):
01603 start = end
01604 end += 4
01605 (length,) = _struct_I.unpack(str[start:end])
01606 start = end
01607 end += length
01608 val1 = str[start:end]
01609 self.ik_request.ik_seed_state.multi_dof_joint_state.frame_ids.append(val1)
01610 start = end
01611 end += 4
01612 (length,) = _struct_I.unpack(str[start:end])
01613 self.ik_request.ik_seed_state.multi_dof_joint_state.child_frame_ids = []
01614 for i in xrange(0, length):
01615 start = end
01616 end += 4
01617 (length,) = _struct_I.unpack(str[start:end])
01618 start = end
01619 end += length
01620 val1 = str[start:end]
01621 self.ik_request.ik_seed_state.multi_dof_joint_state.child_frame_ids.append(val1)
01622 start = end
01623 end += 4
01624 (length,) = _struct_I.unpack(str[start:end])
01625 self.ik_request.ik_seed_state.multi_dof_joint_state.poses = []
01626 for i in xrange(0, length):
01627 val1 = geometry_msgs.msg.Pose()
01628 _v97 = val1.position
01629 _x = _v97
01630 start = end
01631 end += 24
01632 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01633 _v98 = val1.orientation
01634 _x = _v98
01635 start = end
01636 end += 32
01637 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01638 self.ik_request.ik_seed_state.multi_dof_joint_state.poses.append(val1)
01639 _x = self
01640 start = end
01641 end += 12
01642 (_x.ik_request.robot_state.joint_state.header.seq, _x.ik_request.robot_state.joint_state.header.stamp.secs, _x.ik_request.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01643 start = end
01644 end += 4
01645 (length,) = _struct_I.unpack(str[start:end])
01646 start = end
01647 end += length
01648 self.ik_request.robot_state.joint_state.header.frame_id = str[start:end]
01649 start = end
01650 end += 4
01651 (length,) = _struct_I.unpack(str[start:end])
01652 self.ik_request.robot_state.joint_state.name = []
01653 for i in xrange(0, length):
01654 start = end
01655 end += 4
01656 (length,) = _struct_I.unpack(str[start:end])
01657 start = end
01658 end += length
01659 val1 = str[start:end]
01660 self.ik_request.robot_state.joint_state.name.append(val1)
01661 start = end
01662 end += 4
01663 (length,) = _struct_I.unpack(str[start:end])
01664 pattern = '<%sd'%length
01665 start = end
01666 end += struct.calcsize(pattern)
01667 self.ik_request.robot_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01668 start = end
01669 end += 4
01670 (length,) = _struct_I.unpack(str[start:end])
01671 pattern = '<%sd'%length
01672 start = end
01673 end += struct.calcsize(pattern)
01674 self.ik_request.robot_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01675 start = end
01676 end += 4
01677 (length,) = _struct_I.unpack(str[start:end])
01678 pattern = '<%sd'%length
01679 start = end
01680 end += struct.calcsize(pattern)
01681 self.ik_request.robot_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01682 _x = self
01683 start = end
01684 end += 8
01685 (_x.ik_request.robot_state.multi_dof_joint_state.stamp.secs, _x.ik_request.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01686 start = end
01687 end += 4
01688 (length,) = _struct_I.unpack(str[start:end])
01689 self.ik_request.robot_state.multi_dof_joint_state.joint_names = []
01690 for i in xrange(0, length):
01691 start = end
01692 end += 4
01693 (length,) = _struct_I.unpack(str[start:end])
01694 start = end
01695 end += length
01696 val1 = str[start:end]
01697 self.ik_request.robot_state.multi_dof_joint_state.joint_names.append(val1)
01698 start = end
01699 end += 4
01700 (length,) = _struct_I.unpack(str[start:end])
01701 self.ik_request.robot_state.multi_dof_joint_state.frame_ids = []
01702 for i in xrange(0, length):
01703 start = end
01704 end += 4
01705 (length,) = _struct_I.unpack(str[start:end])
01706 start = end
01707 end += length
01708 val1 = str[start:end]
01709 self.ik_request.robot_state.multi_dof_joint_state.frame_ids.append(val1)
01710 start = end
01711 end += 4
01712 (length,) = _struct_I.unpack(str[start:end])
01713 self.ik_request.robot_state.multi_dof_joint_state.child_frame_ids = []
01714 for i in xrange(0, length):
01715 start = end
01716 end += 4
01717 (length,) = _struct_I.unpack(str[start:end])
01718 start = end
01719 end += length
01720 val1 = str[start:end]
01721 self.ik_request.robot_state.multi_dof_joint_state.child_frame_ids.append(val1)
01722 start = end
01723 end += 4
01724 (length,) = _struct_I.unpack(str[start:end])
01725 self.ik_request.robot_state.multi_dof_joint_state.poses = []
01726 for i in xrange(0, length):
01727 val1 = geometry_msgs.msg.Pose()
01728 _v99 = val1.position
01729 _x = _v99
01730 start = end
01731 end += 24
01732 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01733 _v100 = val1.orientation
01734 _x = _v100
01735 start = end
01736 end += 32
01737 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01738 self.ik_request.robot_state.multi_dof_joint_state.poses.append(val1)
01739 start = end
01740 end += 4
01741 (length,) = _struct_I.unpack(str[start:end])
01742 self.allowed_contacts = []
01743 for i in xrange(0, length):
01744 val1 = motion_planning_msgs.msg.AllowedContactSpecification()
01745 start = end
01746 end += 4
01747 (length,) = _struct_I.unpack(str[start:end])
01748 start = end
01749 end += length
01750 val1.name = str[start:end]
01751 _v101 = val1.shape
01752 start = end
01753 end += 1
01754 (_v101.type,) = _struct_b.unpack(str[start:end])
01755 start = end
01756 end += 4
01757 (length,) = _struct_I.unpack(str[start:end])
01758 pattern = '<%sd'%length
01759 start = end
01760 end += struct.calcsize(pattern)
01761 _v101.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01762 start = end
01763 end += 4
01764 (length,) = _struct_I.unpack(str[start:end])
01765 pattern = '<%si'%length
01766 start = end
01767 end += struct.calcsize(pattern)
01768 _v101.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
01769 start = end
01770 end += 4
01771 (length,) = _struct_I.unpack(str[start:end])
01772 _v101.vertices = []
01773 for i in xrange(0, length):
01774 val3 = geometry_msgs.msg.Point()
01775 _x = val3
01776 start = end
01777 end += 24
01778 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01779 _v101.vertices.append(val3)
01780 _v102 = val1.pose_stamped
01781 _v103 = _v102.header
01782 start = end
01783 end += 4
01784 (_v103.seq,) = _struct_I.unpack(str[start:end])
01785 _v104 = _v103.stamp
01786 _x = _v104
01787 start = end
01788 end += 8
01789 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01790 start = end
01791 end += 4
01792 (length,) = _struct_I.unpack(str[start:end])
01793 start = end
01794 end += length
01795 _v103.frame_id = str[start:end]
01796 _v105 = _v102.pose
01797 _v106 = _v105.position
01798 _x = _v106
01799 start = end
01800 end += 24
01801 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01802 _v107 = _v105.orientation
01803 _x = _v107
01804 start = end
01805 end += 32
01806 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01807 start = end
01808 end += 4
01809 (length,) = _struct_I.unpack(str[start:end])
01810 val1.link_names = []
01811 for i in xrange(0, length):
01812 start = end
01813 end += 4
01814 (length,) = _struct_I.unpack(str[start:end])
01815 start = end
01816 end += length
01817 val2 = str[start:end]
01818 val1.link_names.append(val2)
01819 start = end
01820 end += 8
01821 (val1.penetration_depth,) = _struct_d.unpack(str[start:end])
01822 self.allowed_contacts.append(val1)
01823 start = end
01824 end += 4
01825 (length,) = _struct_I.unpack(str[start:end])
01826 self.ordered_collision_operations.collision_operations = []
01827 for i in xrange(0, length):
01828 val1 = motion_planning_msgs.msg.CollisionOperation()
01829 start = end
01830 end += 4
01831 (length,) = _struct_I.unpack(str[start:end])
01832 start = end
01833 end += length
01834 val1.object1 = str[start:end]
01835 start = end
01836 end += 4
01837 (length,) = _struct_I.unpack(str[start:end])
01838 start = end
01839 end += length
01840 val1.object2 = str[start:end]
01841 _x = val1
01842 start = end
01843 end += 12
01844 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
01845 self.ordered_collision_operations.collision_operations.append(val1)
01846 start = end
01847 end += 4
01848 (length,) = _struct_I.unpack(str[start:end])
01849 self.link_padding = []
01850 for i in xrange(0, length):
01851 val1 = motion_planning_msgs.msg.LinkPadding()
01852 start = end
01853 end += 4
01854 (length,) = _struct_I.unpack(str[start:end])
01855 start = end
01856 end += length
01857 val1.link_name = str[start:end]
01858 start = end
01859 end += 8
01860 (val1.padding,) = _struct_d.unpack(str[start:end])
01861 self.link_padding.append(val1)
01862 start = end
01863 end += 4
01864 (length,) = _struct_I.unpack(str[start:end])
01865 self.constraints.joint_constraints = []
01866 for i in xrange(0, length):
01867 val1 = motion_planning_msgs.msg.JointConstraint()
01868 start = end
01869 end += 4
01870 (length,) = _struct_I.unpack(str[start:end])
01871 start = end
01872 end += length
01873 val1.joint_name = str[start:end]
01874 _x = val1
01875 start = end
01876 end += 32
01877 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
01878 self.constraints.joint_constraints.append(val1)
01879 start = end
01880 end += 4
01881 (length,) = _struct_I.unpack(str[start:end])
01882 self.constraints.position_constraints = []
01883 for i in xrange(0, length):
01884 val1 = motion_planning_msgs.msg.PositionConstraint()
01885 _v108 = val1.header
01886 start = end
01887 end += 4
01888 (_v108.seq,) = _struct_I.unpack(str[start:end])
01889 _v109 = _v108.stamp
01890 _x = _v109
01891 start = end
01892 end += 8
01893 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01894 start = end
01895 end += 4
01896 (length,) = _struct_I.unpack(str[start:end])
01897 start = end
01898 end += length
01899 _v108.frame_id = str[start:end]
01900 start = end
01901 end += 4
01902 (length,) = _struct_I.unpack(str[start:end])
01903 start = end
01904 end += length
01905 val1.link_name = str[start:end]
01906 _v110 = val1.target_point_offset
01907 _x = _v110
01908 start = end
01909 end += 24
01910 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01911 _v111 = val1.position
01912 _x = _v111
01913 start = end
01914 end += 24
01915 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01916 _v112 = val1.constraint_region_shape
01917 start = end
01918 end += 1
01919 (_v112.type,) = _struct_b.unpack(str[start:end])
01920 start = end
01921 end += 4
01922 (length,) = _struct_I.unpack(str[start:end])
01923 pattern = '<%sd'%length
01924 start = end
01925 end += struct.calcsize(pattern)
01926 _v112.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01927 start = end
01928 end += 4
01929 (length,) = _struct_I.unpack(str[start:end])
01930 pattern = '<%si'%length
01931 start = end
01932 end += struct.calcsize(pattern)
01933 _v112.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
01934 start = end
01935 end += 4
01936 (length,) = _struct_I.unpack(str[start:end])
01937 _v112.vertices = []
01938 for i in xrange(0, length):
01939 val3 = geometry_msgs.msg.Point()
01940 _x = val3
01941 start = end
01942 end += 24
01943 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01944 _v112.vertices.append(val3)
01945 _v113 = val1.constraint_region_orientation
01946 _x = _v113
01947 start = end
01948 end += 32
01949 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01950 start = end
01951 end += 8
01952 (val1.weight,) = _struct_d.unpack(str[start:end])
01953 self.constraints.position_constraints.append(val1)
01954 start = end
01955 end += 4
01956 (length,) = _struct_I.unpack(str[start:end])
01957 self.constraints.orientation_constraints = []
01958 for i in xrange(0, length):
01959 val1 = motion_planning_msgs.msg.OrientationConstraint()
01960 _v114 = val1.header
01961 start = end
01962 end += 4
01963 (_v114.seq,) = _struct_I.unpack(str[start:end])
01964 _v115 = _v114.stamp
01965 _x = _v115
01966 start = end
01967 end += 8
01968 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01969 start = end
01970 end += 4
01971 (length,) = _struct_I.unpack(str[start:end])
01972 start = end
01973 end += length
01974 _v114.frame_id = str[start:end]
01975 start = end
01976 end += 4
01977 (length,) = _struct_I.unpack(str[start:end])
01978 start = end
01979 end += length
01980 val1.link_name = str[start:end]
01981 start = end
01982 end += 4
01983 (val1.type,) = _struct_i.unpack(str[start:end])
01984 _v116 = val1.orientation
01985 _x = _v116
01986 start = end
01987 end += 32
01988 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01989 _x = val1
01990 start = end
01991 end += 32
01992 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
01993 self.constraints.orientation_constraints.append(val1)
01994 start = end
01995 end += 4
01996 (length,) = _struct_I.unpack(str[start:end])
01997 self.constraints.visibility_constraints = []
01998 for i in xrange(0, length):
01999 val1 = motion_planning_msgs.msg.VisibilityConstraint()
02000 _v117 = val1.header
02001 start = end
02002 end += 4
02003 (_v117.seq,) = _struct_I.unpack(str[start:end])
02004 _v118 = _v117.stamp
02005 _x = _v118
02006 start = end
02007 end += 8
02008 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02009 start = end
02010 end += 4
02011 (length,) = _struct_I.unpack(str[start:end])
02012 start = end
02013 end += length
02014 _v117.frame_id = str[start:end]
02015 _v119 = val1.target
02016 _v120 = _v119.header
02017 start = end
02018 end += 4
02019 (_v120.seq,) = _struct_I.unpack(str[start:end])
02020 _v121 = _v120.stamp
02021 _x = _v121
02022 start = end
02023 end += 8
02024 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02025 start = end
02026 end += 4
02027 (length,) = _struct_I.unpack(str[start:end])
02028 start = end
02029 end += length
02030 _v120.frame_id = str[start:end]
02031 _v122 = _v119.point
02032 _x = _v122
02033 start = end
02034 end += 24
02035 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02036 _v123 = val1.sensor_pose
02037 _v124 = _v123.header
02038 start = end
02039 end += 4
02040 (_v124.seq,) = _struct_I.unpack(str[start:end])
02041 _v125 = _v124.stamp
02042 _x = _v125
02043 start = end
02044 end += 8
02045 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02046 start = end
02047 end += 4
02048 (length,) = _struct_I.unpack(str[start:end])
02049 start = end
02050 end += length
02051 _v124.frame_id = str[start:end]
02052 _v126 = _v123.pose
02053 _v127 = _v126.position
02054 _x = _v127
02055 start = end
02056 end += 24
02057 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02058 _v128 = _v126.orientation
02059 _x = _v128
02060 start = end
02061 end += 32
02062 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02063 start = end
02064 end += 8
02065 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
02066 self.constraints.visibility_constraints.append(val1)
02067 _x = self
02068 start = end
02069 end += 8
02070 (_x.timeout.secs, _x.timeout.nsecs,) = _struct_2i.unpack(str[start:end])
02071 self.timeout.canon()
02072 return self
02073 except struct.error, e:
02074 raise roslib.message.DeserializationError(e)
02075
02076 _struct_I = roslib.message.struct_I
02077 _struct_b = struct.Struct("<b")
02078 _struct_d = struct.Struct("<d")
02079 _struct_di = struct.Struct("<di")
02080 _struct_2I = struct.Struct("<2I")
02081 _struct_2i = struct.Struct("<2i")
02082 _struct_i = struct.Struct("<i")
02083 _struct_3I = struct.Struct("<3I")
02084 _struct_4d = struct.Struct("<4d")
02085 _struct_7d3I = struct.Struct("<7d3I")
02086 _struct_3d = struct.Struct("<3d")
02087 """autogenerated by genmsg_py from GetConstraintAwarePositionIKResponse.msg. Do not edit."""
02088 import roslib.message
02089 import struct
02090
02091 import geometry_msgs.msg
02092 import roslib.rostime
02093 import std_msgs.msg
02094 import motion_planning_msgs.msg
02095 import sensor_msgs.msg
02096
02097 class GetConstraintAwarePositionIKResponse(roslib.message.Message):
02098 _md5sum = "5a8bbc4eb2775fe00cbd09858fd3dc65"
02099 _type = "kinematics_msgs/GetConstraintAwarePositionIKResponse"
02100 _has_header = False
02101 _full_text = """
02102 motion_planning_msgs/RobotState solution
02103 motion_planning_msgs/ArmNavigationErrorCodes error_code
02104
02105
02106 ================================================================================
02107 MSG: motion_planning_msgs/RobotState
02108 # This message contains information about the robot state, i.e. the positions of its joints and links
02109 sensor_msgs/JointState joint_state
02110 motion_planning_msgs/MultiDOFJointState multi_dof_joint_state
02111 ================================================================================
02112 MSG: sensor_msgs/JointState
02113 # This is a message that holds data to describe the state of a set of torque controlled joints.
02114 #
02115 # The state of each joint (revolute or prismatic) is defined by:
02116 # * the position of the joint (rad or m),
02117 # * the velocity of the joint (rad/s or m/s) and
02118 # * the effort that is applied in the joint (Nm or N).
02119 #
02120 # Each joint is uniquely identified by its name
02121 # The header specifies the time at which the joint states were recorded. All the joint states
02122 # in one message have to be recorded at the same time.
02123 #
02124 # This message consists of a multiple arrays, one for each part of the joint state.
02125 # The goal is to make each of the fields optional. When e.g. your joints have no
02126 # effort associated with them, you can leave the effort array empty.
02127 #
02128 # All arrays in this message should have the same size, or be empty.
02129 # This is the only way to uniquely associate the joint name with the correct
02130 # states.
02131
02132
02133 Header header
02134
02135 string[] name
02136 float64[] position
02137 float64[] velocity
02138 float64[] effort
02139
02140 ================================================================================
02141 MSG: std_msgs/Header
02142 # Standard metadata for higher-level stamped data types.
02143 # This is generally used to communicate timestamped data
02144 # in a particular coordinate frame.
02145 #
02146 # sequence ID: consecutively increasing ID
02147 uint32 seq
02148 #Two-integer timestamp that is expressed as:
02149 # * stamp.secs: seconds (stamp_secs) since epoch
02150 # * stamp.nsecs: nanoseconds since stamp_secs
02151 # time-handling sugar is provided by the client library
02152 time stamp
02153 #Frame this data is associated with
02154 # 0: no frame
02155 # 1: global frame
02156 string frame_id
02157
02158 ================================================================================
02159 MSG: motion_planning_msgs/MultiDOFJointState
02160 #A representation of a multi-dof joint state
02161 time stamp
02162 string[] joint_names
02163 string[] frame_ids
02164 string[] child_frame_ids
02165 geometry_msgs/Pose[] poses
02166
02167 ================================================================================
02168 MSG: geometry_msgs/Pose
02169 # A representation of pose in free space, composed of postion and orientation.
02170 Point position
02171 Quaternion orientation
02172
02173 ================================================================================
02174 MSG: geometry_msgs/Point
02175 # This contains the position of a point in free space
02176 float64 x
02177 float64 y
02178 float64 z
02179
02180 ================================================================================
02181 MSG: geometry_msgs/Quaternion
02182 # This represents an orientation in free space in quaternion form.
02183
02184 float64 x
02185 float64 y
02186 float64 z
02187 float64 w
02188
02189 ================================================================================
02190 MSG: motion_planning_msgs/ArmNavigationErrorCodes
02191 int32 val
02192
02193 # overall behavior
02194 int32 PLANNING_FAILED=-1
02195 int32 SUCCESS=1
02196 int32 TIMED_OUT=-2
02197
02198 # start state errors
02199 int32 START_STATE_IN_COLLISION=-3
02200 int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-4
02201
02202 # goal errors
02203 int32 GOAL_IN_COLLISION=-5
02204 int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-6
02205
02206 # robot state
02207 int32 INVALID_ROBOT_STATE=-7
02208 int32 INCOMPLETE_ROBOT_STATE=-8
02209
02210 # planning request errors
02211 int32 INVALID_PLANNER_ID=-9
02212 int32 INVALID_NUM_PLANNING_ATTEMPTS=-10
02213 int32 INVALID_ALLOWED_PLANNING_TIME=-11
02214 int32 INVALID_GROUP_NAME=-12
02215 int32 INVALID_GOAL_JOINT_CONSTRAINTS=-13
02216 int32 INVALID_GOAL_POSITION_CONSTRAINTS=-14
02217 int32 INVALID_GOAL_ORIENTATION_CONSTRAINTS=-15
02218 int32 INVALID_PATH_JOINT_CONSTRAINTS=-16
02219 int32 INVALID_PATH_POSITION_CONSTRAINTS=-17
02220 int32 INVALID_PATH_ORIENTATION_CONSTRAINTS=-18
02221
02222 # state/trajectory monitor errors
02223 int32 INVALID_TRAJECTORY=-19
02224 int32 INVALID_INDEX=-20
02225 int32 JOINT_LIMITS_VIOLATED=-21
02226 int32 PATH_CONSTRAINTS_VIOLATED=-22
02227 int32 COLLISION_CONSTRAINTS_VIOLATED=-23
02228 int32 GOAL_CONSTRAINTS_VIOLATED=-24
02229 int32 JOINTS_NOT_MOVING=-25
02230 int32 TRAJECTORY_CONTROLLER_FAILED=-26
02231
02232 # system errors
02233 int32 FRAME_TRANSFORM_FAILURE=-27
02234 int32 COLLISION_CHECKING_UNAVAILABLE=-28
02235 int32 ROBOT_STATE_STALE=-29
02236 int32 SENSOR_INFO_STALE=-30
02237
02238 # kinematics errors
02239 int32 NO_IK_SOLUTION=-31
02240 int32 INVALID_LINK_NAME=-32
02241 int32 IK_LINK_IN_COLLISION=-33
02242 int32 NO_FK_SOLUTION=-34
02243 int32 KINEMATICS_STATE_IN_COLLISION=-35
02244
02245 # general errors
02246 int32 INVALID_TIMEOUT=-36
02247
02248
02249 """
02250 __slots__ = ['solution','error_code']
02251 _slot_types = ['motion_planning_msgs/RobotState','motion_planning_msgs/ArmNavigationErrorCodes']
02252
02253 def __init__(self, *args, **kwds):
02254 """
02255 Constructor. Any message fields that are implicitly/explicitly
02256 set to None will be assigned a default value. The recommend
02257 use is keyword arguments as this is more robust to future message
02258 changes. You cannot mix in-order arguments and keyword arguments.
02259
02260 The available fields are:
02261 solution,error_code
02262
02263 @param args: complete set of field values, in .msg order
02264 @param kwds: use keyword arguments corresponding to message field names
02265 to set specific fields.
02266 """
02267 if args or kwds:
02268 super(GetConstraintAwarePositionIKResponse, self).__init__(*args, **kwds)
02269
02270 if self.solution is None:
02271 self.solution = motion_planning_msgs.msg.RobotState()
02272 if self.error_code is None:
02273 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
02274 else:
02275 self.solution = motion_planning_msgs.msg.RobotState()
02276 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
02277
02278 def _get_types(self):
02279 """
02280 internal API method
02281 """
02282 return self._slot_types
02283
02284 def serialize(self, buff):
02285 """
02286 serialize message into buffer
02287 @param buff: buffer
02288 @type buff: StringIO
02289 """
02290 try:
02291 _x = self
02292 buff.write(_struct_3I.pack(_x.solution.joint_state.header.seq, _x.solution.joint_state.header.stamp.secs, _x.solution.joint_state.header.stamp.nsecs))
02293 _x = self.solution.joint_state.header.frame_id
02294 length = len(_x)
02295 buff.write(struct.pack('<I%ss'%length, length, _x))
02296 length = len(self.solution.joint_state.name)
02297 buff.write(_struct_I.pack(length))
02298 for val1 in self.solution.joint_state.name:
02299 length = len(val1)
02300 buff.write(struct.pack('<I%ss'%length, length, val1))
02301 length = len(self.solution.joint_state.position)
02302 buff.write(_struct_I.pack(length))
02303 pattern = '<%sd'%length
02304 buff.write(struct.pack(pattern, *self.solution.joint_state.position))
02305 length = len(self.solution.joint_state.velocity)
02306 buff.write(_struct_I.pack(length))
02307 pattern = '<%sd'%length
02308 buff.write(struct.pack(pattern, *self.solution.joint_state.velocity))
02309 length = len(self.solution.joint_state.effort)
02310 buff.write(_struct_I.pack(length))
02311 pattern = '<%sd'%length
02312 buff.write(struct.pack(pattern, *self.solution.joint_state.effort))
02313 _x = self
02314 buff.write(_struct_2I.pack(_x.solution.multi_dof_joint_state.stamp.secs, _x.solution.multi_dof_joint_state.stamp.nsecs))
02315 length = len(self.solution.multi_dof_joint_state.joint_names)
02316 buff.write(_struct_I.pack(length))
02317 for val1 in self.solution.multi_dof_joint_state.joint_names:
02318 length = len(val1)
02319 buff.write(struct.pack('<I%ss'%length, length, val1))
02320 length = len(self.solution.multi_dof_joint_state.frame_ids)
02321 buff.write(_struct_I.pack(length))
02322 for val1 in self.solution.multi_dof_joint_state.frame_ids:
02323 length = len(val1)
02324 buff.write(struct.pack('<I%ss'%length, length, val1))
02325 length = len(self.solution.multi_dof_joint_state.child_frame_ids)
02326 buff.write(_struct_I.pack(length))
02327 for val1 in self.solution.multi_dof_joint_state.child_frame_ids:
02328 length = len(val1)
02329 buff.write(struct.pack('<I%ss'%length, length, val1))
02330 length = len(self.solution.multi_dof_joint_state.poses)
02331 buff.write(_struct_I.pack(length))
02332 for val1 in self.solution.multi_dof_joint_state.poses:
02333 _v129 = val1.position
02334 _x = _v129
02335 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02336 _v130 = val1.orientation
02337 _x = _v130
02338 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02339 buff.write(_struct_i.pack(self.error_code.val))
02340 except struct.error, se: self._check_types(se)
02341 except TypeError, te: self._check_types(te)
02342
02343 def deserialize(self, str):
02344 """
02345 unpack serialized message in str into this message instance
02346 @param str: byte array of serialized message
02347 @type str: str
02348 """
02349 try:
02350 if self.solution is None:
02351 self.solution = motion_planning_msgs.msg.RobotState()
02352 if self.error_code is None:
02353 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
02354 end = 0
02355 _x = self
02356 start = end
02357 end += 12
02358 (_x.solution.joint_state.header.seq, _x.solution.joint_state.header.stamp.secs, _x.solution.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02359 start = end
02360 end += 4
02361 (length,) = _struct_I.unpack(str[start:end])
02362 start = end
02363 end += length
02364 self.solution.joint_state.header.frame_id = str[start:end]
02365 start = end
02366 end += 4
02367 (length,) = _struct_I.unpack(str[start:end])
02368 self.solution.joint_state.name = []
02369 for i in xrange(0, length):
02370 start = end
02371 end += 4
02372 (length,) = _struct_I.unpack(str[start:end])
02373 start = end
02374 end += length
02375 val1 = str[start:end]
02376 self.solution.joint_state.name.append(val1)
02377 start = end
02378 end += 4
02379 (length,) = _struct_I.unpack(str[start:end])
02380 pattern = '<%sd'%length
02381 start = end
02382 end += struct.calcsize(pattern)
02383 self.solution.joint_state.position = struct.unpack(pattern, str[start:end])
02384 start = end
02385 end += 4
02386 (length,) = _struct_I.unpack(str[start:end])
02387 pattern = '<%sd'%length
02388 start = end
02389 end += struct.calcsize(pattern)
02390 self.solution.joint_state.velocity = struct.unpack(pattern, str[start:end])
02391 start = end
02392 end += 4
02393 (length,) = _struct_I.unpack(str[start:end])
02394 pattern = '<%sd'%length
02395 start = end
02396 end += struct.calcsize(pattern)
02397 self.solution.joint_state.effort = struct.unpack(pattern, str[start:end])
02398 _x = self
02399 start = end
02400 end += 8
02401 (_x.solution.multi_dof_joint_state.stamp.secs, _x.solution.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
02402 start = end
02403 end += 4
02404 (length,) = _struct_I.unpack(str[start:end])
02405 self.solution.multi_dof_joint_state.joint_names = []
02406 for i in xrange(0, length):
02407 start = end
02408 end += 4
02409 (length,) = _struct_I.unpack(str[start:end])
02410 start = end
02411 end += length
02412 val1 = str[start:end]
02413 self.solution.multi_dof_joint_state.joint_names.append(val1)
02414 start = end
02415 end += 4
02416 (length,) = _struct_I.unpack(str[start:end])
02417 self.solution.multi_dof_joint_state.frame_ids = []
02418 for i in xrange(0, length):
02419 start = end
02420 end += 4
02421 (length,) = _struct_I.unpack(str[start:end])
02422 start = end
02423 end += length
02424 val1 = str[start:end]
02425 self.solution.multi_dof_joint_state.frame_ids.append(val1)
02426 start = end
02427 end += 4
02428 (length,) = _struct_I.unpack(str[start:end])
02429 self.solution.multi_dof_joint_state.child_frame_ids = []
02430 for i in xrange(0, length):
02431 start = end
02432 end += 4
02433 (length,) = _struct_I.unpack(str[start:end])
02434 start = end
02435 end += length
02436 val1 = str[start:end]
02437 self.solution.multi_dof_joint_state.child_frame_ids.append(val1)
02438 start = end
02439 end += 4
02440 (length,) = _struct_I.unpack(str[start:end])
02441 self.solution.multi_dof_joint_state.poses = []
02442 for i in xrange(0, length):
02443 val1 = geometry_msgs.msg.Pose()
02444 _v131 = val1.position
02445 _x = _v131
02446 start = end
02447 end += 24
02448 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02449 _v132 = val1.orientation
02450 _x = _v132
02451 start = end
02452 end += 32
02453 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02454 self.solution.multi_dof_joint_state.poses.append(val1)
02455 start = end
02456 end += 4
02457 (self.error_code.val,) = _struct_i.unpack(str[start:end])
02458 return self
02459 except struct.error, e:
02460 raise roslib.message.DeserializationError(e)
02461
02462
02463 def serialize_numpy(self, buff, numpy):
02464 """
02465 serialize message with numpy array types into buffer
02466 @param buff: buffer
02467 @type buff: StringIO
02468 @param numpy: numpy python module
02469 @type numpy module
02470 """
02471 try:
02472 _x = self
02473 buff.write(_struct_3I.pack(_x.solution.joint_state.header.seq, _x.solution.joint_state.header.stamp.secs, _x.solution.joint_state.header.stamp.nsecs))
02474 _x = self.solution.joint_state.header.frame_id
02475 length = len(_x)
02476 buff.write(struct.pack('<I%ss'%length, length, _x))
02477 length = len(self.solution.joint_state.name)
02478 buff.write(_struct_I.pack(length))
02479 for val1 in self.solution.joint_state.name:
02480 length = len(val1)
02481 buff.write(struct.pack('<I%ss'%length, length, val1))
02482 length = len(self.solution.joint_state.position)
02483 buff.write(_struct_I.pack(length))
02484 pattern = '<%sd'%length
02485 buff.write(self.solution.joint_state.position.tostring())
02486 length = len(self.solution.joint_state.velocity)
02487 buff.write(_struct_I.pack(length))
02488 pattern = '<%sd'%length
02489 buff.write(self.solution.joint_state.velocity.tostring())
02490 length = len(self.solution.joint_state.effort)
02491 buff.write(_struct_I.pack(length))
02492 pattern = '<%sd'%length
02493 buff.write(self.solution.joint_state.effort.tostring())
02494 _x = self
02495 buff.write(_struct_2I.pack(_x.solution.multi_dof_joint_state.stamp.secs, _x.solution.multi_dof_joint_state.stamp.nsecs))
02496 length = len(self.solution.multi_dof_joint_state.joint_names)
02497 buff.write(_struct_I.pack(length))
02498 for val1 in self.solution.multi_dof_joint_state.joint_names:
02499 length = len(val1)
02500 buff.write(struct.pack('<I%ss'%length, length, val1))
02501 length = len(self.solution.multi_dof_joint_state.frame_ids)
02502 buff.write(_struct_I.pack(length))
02503 for val1 in self.solution.multi_dof_joint_state.frame_ids:
02504 length = len(val1)
02505 buff.write(struct.pack('<I%ss'%length, length, val1))
02506 length = len(self.solution.multi_dof_joint_state.child_frame_ids)
02507 buff.write(_struct_I.pack(length))
02508 for val1 in self.solution.multi_dof_joint_state.child_frame_ids:
02509 length = len(val1)
02510 buff.write(struct.pack('<I%ss'%length, length, val1))
02511 length = len(self.solution.multi_dof_joint_state.poses)
02512 buff.write(_struct_I.pack(length))
02513 for val1 in self.solution.multi_dof_joint_state.poses:
02514 _v133 = val1.position
02515 _x = _v133
02516 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02517 _v134 = val1.orientation
02518 _x = _v134
02519 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02520 buff.write(_struct_i.pack(self.error_code.val))
02521 except struct.error, se: self._check_types(se)
02522 except TypeError, te: self._check_types(te)
02523
02524 def deserialize_numpy(self, str, numpy):
02525 """
02526 unpack serialized message in str into this message instance using numpy for array types
02527 @param str: byte array of serialized message
02528 @type str: str
02529 @param numpy: numpy python module
02530 @type numpy: module
02531 """
02532 try:
02533 if self.solution is None:
02534 self.solution = motion_planning_msgs.msg.RobotState()
02535 if self.error_code is None:
02536 self.error_code = motion_planning_msgs.msg.ArmNavigationErrorCodes()
02537 end = 0
02538 _x = self
02539 start = end
02540 end += 12
02541 (_x.solution.joint_state.header.seq, _x.solution.joint_state.header.stamp.secs, _x.solution.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02542 start = end
02543 end += 4
02544 (length,) = _struct_I.unpack(str[start:end])
02545 start = end
02546 end += length
02547 self.solution.joint_state.header.frame_id = str[start:end]
02548 start = end
02549 end += 4
02550 (length,) = _struct_I.unpack(str[start:end])
02551 self.solution.joint_state.name = []
02552 for i in xrange(0, length):
02553 start = end
02554 end += 4
02555 (length,) = _struct_I.unpack(str[start:end])
02556 start = end
02557 end += length
02558 val1 = str[start:end]
02559 self.solution.joint_state.name.append(val1)
02560 start = end
02561 end += 4
02562 (length,) = _struct_I.unpack(str[start:end])
02563 pattern = '<%sd'%length
02564 start = end
02565 end += struct.calcsize(pattern)
02566 self.solution.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02567 start = end
02568 end += 4
02569 (length,) = _struct_I.unpack(str[start:end])
02570 pattern = '<%sd'%length
02571 start = end
02572 end += struct.calcsize(pattern)
02573 self.solution.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02574 start = end
02575 end += 4
02576 (length,) = _struct_I.unpack(str[start:end])
02577 pattern = '<%sd'%length
02578 start = end
02579 end += struct.calcsize(pattern)
02580 self.solution.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02581 _x = self
02582 start = end
02583 end += 8
02584 (_x.solution.multi_dof_joint_state.stamp.secs, _x.solution.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
02585 start = end
02586 end += 4
02587 (length,) = _struct_I.unpack(str[start:end])
02588 self.solution.multi_dof_joint_state.joint_names = []
02589 for i in xrange(0, length):
02590 start = end
02591 end += 4
02592 (length,) = _struct_I.unpack(str[start:end])
02593 start = end
02594 end += length
02595 val1 = str[start:end]
02596 self.solution.multi_dof_joint_state.joint_names.append(val1)
02597 start = end
02598 end += 4
02599 (length,) = _struct_I.unpack(str[start:end])
02600 self.solution.multi_dof_joint_state.frame_ids = []
02601 for i in xrange(0, length):
02602 start = end
02603 end += 4
02604 (length,) = _struct_I.unpack(str[start:end])
02605 start = end
02606 end += length
02607 val1 = str[start:end]
02608 self.solution.multi_dof_joint_state.frame_ids.append(val1)
02609 start = end
02610 end += 4
02611 (length,) = _struct_I.unpack(str[start:end])
02612 self.solution.multi_dof_joint_state.child_frame_ids = []
02613 for i in xrange(0, length):
02614 start = end
02615 end += 4
02616 (length,) = _struct_I.unpack(str[start:end])
02617 start = end
02618 end += length
02619 val1 = str[start:end]
02620 self.solution.multi_dof_joint_state.child_frame_ids.append(val1)
02621 start = end
02622 end += 4
02623 (length,) = _struct_I.unpack(str[start:end])
02624 self.solution.multi_dof_joint_state.poses = []
02625 for i in xrange(0, length):
02626 val1 = geometry_msgs.msg.Pose()
02627 _v135 = val1.position
02628 _x = _v135
02629 start = end
02630 end += 24
02631 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02632 _v136 = val1.orientation
02633 _x = _v136
02634 start = end
02635 end += 32
02636 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02637 self.solution.multi_dof_joint_state.poses.append(val1)
02638 start = end
02639 end += 4
02640 (self.error_code.val,) = _struct_i.unpack(str[start:end])
02641 return self
02642 except struct.error, e:
02643 raise roslib.message.DeserializationError(e)
02644
02645 _struct_I = roslib.message.struct_I
02646 _struct_i = struct.Struct("<i")
02647 _struct_4d = struct.Struct("<4d")
02648 _struct_3I = struct.Struct("<3I")
02649 _struct_2I = struct.Struct("<2I")
02650 _struct_3d = struct.Struct("<3d")
02651 class GetConstraintAwarePositionIK(roslib.message.ServiceDefinition):
02652 _type = 'kinematics_msgs/GetConstraintAwarePositionIK'
02653 _md5sum = '41929f5e39d48851e0d642a31bd2c6d2'
02654 _request_class = GetConstraintAwarePositionIKRequest
02655 _response_class = GetConstraintAwarePositionIKResponse