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