00001 """autogenerated by genpy from hrl_haptic_manipulation_in_clutter_msgs/RobotHapticState.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006
00007 import geometry_msgs.msg
00008 import hrl_haptic_manipulation_in_clutter_msgs.msg
00009 import std_msgs.msg
00010
00011 class RobotHapticState(genpy.Message):
00012 _md5sum = "4ef8a46b3c9c12684f840f17a9ab457f"
00013 _type = "hrl_haptic_manipulation_in_clutter_msgs/RobotHapticState"
00014 _has_header = True
00015 _full_text = """Header header
00016
00017 string[] joint_names
00018 float64[] joint_angles
00019 float64[] desired_joint_angles
00020 float64[] joint_velocities
00021 float64[] joint_stiffness
00022 float64[] joint_damping
00023
00024 geometry_msgs/Pose torso_pose
00025 geometry_msgs/Pose hand_pose
00026
00027 std_msgs/Float64MultiArray end_effector_jacobian
00028 std_msgs/Float64MultiArray contact_jacobians
00029
00030 hrl_haptic_manipulation_in_clutter_msgs/TaxelArray[] skins
00031
00032
00033
00034
00035 ================================================================================
00036 MSG: std_msgs/Header
00037 # Standard metadata for higher-level stamped data types.
00038 # This is generally used to communicate timestamped data
00039 # in a particular coordinate frame.
00040 #
00041 # sequence ID: consecutively increasing ID
00042 uint32 seq
00043 #Two-integer timestamp that is expressed as:
00044 # * stamp.secs: seconds (stamp_secs) since epoch
00045 # * stamp.nsecs: nanoseconds since stamp_secs
00046 # time-handling sugar is provided by the client library
00047 time stamp
00048 #Frame this data is associated with
00049 # 0: no frame
00050 # 1: global frame
00051 string frame_id
00052
00053 ================================================================================
00054 MSG: geometry_msgs/Pose
00055 # A representation of pose in free space, composed of postion and orientation.
00056 Point position
00057 Quaternion orientation
00058
00059 ================================================================================
00060 MSG: geometry_msgs/Point
00061 # This contains the position of a point in free space
00062 float64 x
00063 float64 y
00064 float64 z
00065
00066 ================================================================================
00067 MSG: geometry_msgs/Quaternion
00068 # This represents an orientation in free space in quaternion form.
00069
00070 float64 x
00071 float64 y
00072 float64 z
00073 float64 w
00074
00075 ================================================================================
00076 MSG: std_msgs/Float64MultiArray
00077 # Please look at the MultiArrayLayout message definition for
00078 # documentation on all multiarrays.
00079
00080 MultiArrayLayout layout # specification of data layout
00081 float64[] data # array of data
00082
00083
00084 ================================================================================
00085 MSG: std_msgs/MultiArrayLayout
00086 # The multiarray declares a generic multi-dimensional array of a
00087 # particular data type. Dimensions are ordered from outer most
00088 # to inner most.
00089
00090 MultiArrayDimension[] dim # Array of dimension properties
00091 uint32 data_offset # padding bytes at front of data
00092
00093 # Accessors should ALWAYS be written in terms of dimension stride
00094 # and specified outer-most dimension first.
00095 #
00096 # multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]
00097 #
00098 # A standard, 3-channel 640x480 image with interleaved color channels
00099 # would be specified as:
00100 #
00101 # dim[0].label = "height"
00102 # dim[0].size = 480
00103 # dim[0].stride = 3*640*480 = 921600 (note dim[0] stride is just size of image)
00104 # dim[1].label = "width"
00105 # dim[1].size = 640
00106 # dim[1].stride = 3*640 = 1920
00107 # dim[2].label = "channel"
00108 # dim[2].size = 3
00109 # dim[2].stride = 3
00110 #
00111 # multiarray(i,j,k) refers to the ith row, jth column, and kth channel.
00112 ================================================================================
00113 MSG: std_msgs/MultiArrayDimension
00114 string label # label of given dimension
00115 uint32 size # size of given dimension (in type units)
00116 uint32 stride # stride of given dimension
00117 ================================================================================
00118 MSG: hrl_haptic_manipulation_in_clutter_msgs/TaxelArray
00119 Header header
00120
00121 string sensor_type
00122
00123 string[] link_names
00124
00125 float64[] centers_x
00126 float64[] centers_y
00127 float64[] centers_z
00128
00129 float64[] normals_x
00130 float64[] normals_y
00131 float64[] normals_z
00132
00133 float64[] values_x
00134 float64[] values_y
00135 float64[] values_z
00136
00137
00138
00139
00140 """
00141 __slots__ = ['header','joint_names','joint_angles','desired_joint_angles','joint_velocities','joint_stiffness','joint_damping','torso_pose','hand_pose','end_effector_jacobian','contact_jacobians','skins']
00142 _slot_types = ['std_msgs/Header','string[]','float64[]','float64[]','float64[]','float64[]','float64[]','geometry_msgs/Pose','geometry_msgs/Pose','std_msgs/Float64MultiArray','std_msgs/Float64MultiArray','hrl_haptic_manipulation_in_clutter_msgs/TaxelArray[]']
00143
00144 def __init__(self, *args, **kwds):
00145 """
00146 Constructor. Any message fields that are implicitly/explicitly
00147 set to None will be assigned a default value. The recommend
00148 use is keyword arguments as this is more robust to future message
00149 changes. You cannot mix in-order arguments and keyword arguments.
00150
00151 The available fields are:
00152 header,joint_names,joint_angles,desired_joint_angles,joint_velocities,joint_stiffness,joint_damping,torso_pose,hand_pose,end_effector_jacobian,contact_jacobians,skins
00153
00154 :param args: complete set of field values, in .msg order
00155 :param kwds: use keyword arguments corresponding to message field names
00156 to set specific fields.
00157 """
00158 if args or kwds:
00159 super(RobotHapticState, self).__init__(*args, **kwds)
00160
00161 if self.header is None:
00162 self.header = std_msgs.msg.Header()
00163 if self.joint_names is None:
00164 self.joint_names = []
00165 if self.joint_angles is None:
00166 self.joint_angles = []
00167 if self.desired_joint_angles is None:
00168 self.desired_joint_angles = []
00169 if self.joint_velocities is None:
00170 self.joint_velocities = []
00171 if self.joint_stiffness is None:
00172 self.joint_stiffness = []
00173 if self.joint_damping is None:
00174 self.joint_damping = []
00175 if self.torso_pose is None:
00176 self.torso_pose = geometry_msgs.msg.Pose()
00177 if self.hand_pose is None:
00178 self.hand_pose = geometry_msgs.msg.Pose()
00179 if self.end_effector_jacobian is None:
00180 self.end_effector_jacobian = std_msgs.msg.Float64MultiArray()
00181 if self.contact_jacobians is None:
00182 self.contact_jacobians = std_msgs.msg.Float64MultiArray()
00183 if self.skins is None:
00184 self.skins = []
00185 else:
00186 self.header = std_msgs.msg.Header()
00187 self.joint_names = []
00188 self.joint_angles = []
00189 self.desired_joint_angles = []
00190 self.joint_velocities = []
00191 self.joint_stiffness = []
00192 self.joint_damping = []
00193 self.torso_pose = geometry_msgs.msg.Pose()
00194 self.hand_pose = geometry_msgs.msg.Pose()
00195 self.end_effector_jacobian = std_msgs.msg.Float64MultiArray()
00196 self.contact_jacobians = std_msgs.msg.Float64MultiArray()
00197 self.skins = []
00198
00199 def _get_types(self):
00200 """
00201 internal API method
00202 """
00203 return self._slot_types
00204
00205 def serialize(self, buff):
00206 """
00207 serialize message into buffer
00208 :param buff: buffer, ``StringIO``
00209 """
00210 try:
00211 _x = self
00212 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00213 _x = self.header.frame_id
00214 length = len(_x)
00215 if python3 or type(_x) == unicode:
00216 _x = _x.encode('utf-8')
00217 length = len(_x)
00218 buff.write(struct.pack('<I%ss'%length, length, _x))
00219 length = len(self.joint_names)
00220 buff.write(_struct_I.pack(length))
00221 for val1 in self.joint_names:
00222 length = len(val1)
00223 if python3 or type(val1) == unicode:
00224 val1 = val1.encode('utf-8')
00225 length = len(val1)
00226 buff.write(struct.pack('<I%ss'%length, length, val1))
00227 length = len(self.joint_angles)
00228 buff.write(_struct_I.pack(length))
00229 pattern = '<%sd'%length
00230 buff.write(struct.pack(pattern, *self.joint_angles))
00231 length = len(self.desired_joint_angles)
00232 buff.write(_struct_I.pack(length))
00233 pattern = '<%sd'%length
00234 buff.write(struct.pack(pattern, *self.desired_joint_angles))
00235 length = len(self.joint_velocities)
00236 buff.write(_struct_I.pack(length))
00237 pattern = '<%sd'%length
00238 buff.write(struct.pack(pattern, *self.joint_velocities))
00239 length = len(self.joint_stiffness)
00240 buff.write(_struct_I.pack(length))
00241 pattern = '<%sd'%length
00242 buff.write(struct.pack(pattern, *self.joint_stiffness))
00243 length = len(self.joint_damping)
00244 buff.write(_struct_I.pack(length))
00245 pattern = '<%sd'%length
00246 buff.write(struct.pack(pattern, *self.joint_damping))
00247 _x = self
00248 buff.write(_struct_14d.pack(_x.torso_pose.position.x, _x.torso_pose.position.y, _x.torso_pose.position.z, _x.torso_pose.orientation.x, _x.torso_pose.orientation.y, _x.torso_pose.orientation.z, _x.torso_pose.orientation.w, _x.hand_pose.position.x, _x.hand_pose.position.y, _x.hand_pose.position.z, _x.hand_pose.orientation.x, _x.hand_pose.orientation.y, _x.hand_pose.orientation.z, _x.hand_pose.orientation.w))
00249 length = len(self.end_effector_jacobian.layout.dim)
00250 buff.write(_struct_I.pack(length))
00251 for val1 in self.end_effector_jacobian.layout.dim:
00252 _x = val1.label
00253 length = len(_x)
00254 if python3 or type(_x) == unicode:
00255 _x = _x.encode('utf-8')
00256 length = len(_x)
00257 buff.write(struct.pack('<I%ss'%length, length, _x))
00258 _x = val1
00259 buff.write(_struct_2I.pack(_x.size, _x.stride))
00260 buff.write(_struct_I.pack(self.end_effector_jacobian.layout.data_offset))
00261 length = len(self.end_effector_jacobian.data)
00262 buff.write(_struct_I.pack(length))
00263 pattern = '<%sd'%length
00264 buff.write(struct.pack(pattern, *self.end_effector_jacobian.data))
00265 length = len(self.contact_jacobians.layout.dim)
00266 buff.write(_struct_I.pack(length))
00267 for val1 in self.contact_jacobians.layout.dim:
00268 _x = val1.label
00269 length = len(_x)
00270 if python3 or type(_x) == unicode:
00271 _x = _x.encode('utf-8')
00272 length = len(_x)
00273 buff.write(struct.pack('<I%ss'%length, length, _x))
00274 _x = val1
00275 buff.write(_struct_2I.pack(_x.size, _x.stride))
00276 buff.write(_struct_I.pack(self.contact_jacobians.layout.data_offset))
00277 length = len(self.contact_jacobians.data)
00278 buff.write(_struct_I.pack(length))
00279 pattern = '<%sd'%length
00280 buff.write(struct.pack(pattern, *self.contact_jacobians.data))
00281 length = len(self.skins)
00282 buff.write(_struct_I.pack(length))
00283 for val1 in self.skins:
00284 _v1 = val1.header
00285 buff.write(_struct_I.pack(_v1.seq))
00286 _v2 = _v1.stamp
00287 _x = _v2
00288 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00289 _x = _v1.frame_id
00290 length = len(_x)
00291 if python3 or type(_x) == unicode:
00292 _x = _x.encode('utf-8')
00293 length = len(_x)
00294 buff.write(struct.pack('<I%ss'%length, length, _x))
00295 _x = val1.sensor_type
00296 length = len(_x)
00297 if python3 or type(_x) == unicode:
00298 _x = _x.encode('utf-8')
00299 length = len(_x)
00300 buff.write(struct.pack('<I%ss'%length, length, _x))
00301 length = len(val1.link_names)
00302 buff.write(_struct_I.pack(length))
00303 for val2 in val1.link_names:
00304 length = len(val2)
00305 if python3 or type(val2) == unicode:
00306 val2 = val2.encode('utf-8')
00307 length = len(val2)
00308 buff.write(struct.pack('<I%ss'%length, length, val2))
00309 length = len(val1.centers_x)
00310 buff.write(_struct_I.pack(length))
00311 pattern = '<%sd'%length
00312 buff.write(struct.pack(pattern, *val1.centers_x))
00313 length = len(val1.centers_y)
00314 buff.write(_struct_I.pack(length))
00315 pattern = '<%sd'%length
00316 buff.write(struct.pack(pattern, *val1.centers_y))
00317 length = len(val1.centers_z)
00318 buff.write(_struct_I.pack(length))
00319 pattern = '<%sd'%length
00320 buff.write(struct.pack(pattern, *val1.centers_z))
00321 length = len(val1.normals_x)
00322 buff.write(_struct_I.pack(length))
00323 pattern = '<%sd'%length
00324 buff.write(struct.pack(pattern, *val1.normals_x))
00325 length = len(val1.normals_y)
00326 buff.write(_struct_I.pack(length))
00327 pattern = '<%sd'%length
00328 buff.write(struct.pack(pattern, *val1.normals_y))
00329 length = len(val1.normals_z)
00330 buff.write(_struct_I.pack(length))
00331 pattern = '<%sd'%length
00332 buff.write(struct.pack(pattern, *val1.normals_z))
00333 length = len(val1.values_x)
00334 buff.write(_struct_I.pack(length))
00335 pattern = '<%sd'%length
00336 buff.write(struct.pack(pattern, *val1.values_x))
00337 length = len(val1.values_y)
00338 buff.write(_struct_I.pack(length))
00339 pattern = '<%sd'%length
00340 buff.write(struct.pack(pattern, *val1.values_y))
00341 length = len(val1.values_z)
00342 buff.write(_struct_I.pack(length))
00343 pattern = '<%sd'%length
00344 buff.write(struct.pack(pattern, *val1.values_z))
00345 except struct.error as se: self._check_types(se)
00346 except TypeError as te: self._check_types(te)
00347
00348 def deserialize(self, str):
00349 """
00350 unpack serialized message in str into this message instance
00351 :param str: byte array of serialized message, ``str``
00352 """
00353 try:
00354 if self.header is None:
00355 self.header = std_msgs.msg.Header()
00356 if self.torso_pose is None:
00357 self.torso_pose = geometry_msgs.msg.Pose()
00358 if self.hand_pose is None:
00359 self.hand_pose = geometry_msgs.msg.Pose()
00360 if self.end_effector_jacobian is None:
00361 self.end_effector_jacobian = std_msgs.msg.Float64MultiArray()
00362 if self.contact_jacobians is None:
00363 self.contact_jacobians = std_msgs.msg.Float64MultiArray()
00364 if self.skins is None:
00365 self.skins = None
00366 end = 0
00367 _x = self
00368 start = end
00369 end += 12
00370 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00371 start = end
00372 end += 4
00373 (length,) = _struct_I.unpack(str[start:end])
00374 start = end
00375 end += length
00376 if python3:
00377 self.header.frame_id = str[start:end].decode('utf-8')
00378 else:
00379 self.header.frame_id = str[start:end]
00380 start = end
00381 end += 4
00382 (length,) = _struct_I.unpack(str[start:end])
00383 self.joint_names = []
00384 for i in range(0, length):
00385 start = end
00386 end += 4
00387 (length,) = _struct_I.unpack(str[start:end])
00388 start = end
00389 end += length
00390 if python3:
00391 val1 = str[start:end].decode('utf-8')
00392 else:
00393 val1 = str[start:end]
00394 self.joint_names.append(val1)
00395 start = end
00396 end += 4
00397 (length,) = _struct_I.unpack(str[start:end])
00398 pattern = '<%sd'%length
00399 start = end
00400 end += struct.calcsize(pattern)
00401 self.joint_angles = struct.unpack(pattern, str[start:end])
00402 start = end
00403 end += 4
00404 (length,) = _struct_I.unpack(str[start:end])
00405 pattern = '<%sd'%length
00406 start = end
00407 end += struct.calcsize(pattern)
00408 self.desired_joint_angles = struct.unpack(pattern, str[start:end])
00409 start = end
00410 end += 4
00411 (length,) = _struct_I.unpack(str[start:end])
00412 pattern = '<%sd'%length
00413 start = end
00414 end += struct.calcsize(pattern)
00415 self.joint_velocities = struct.unpack(pattern, str[start:end])
00416 start = end
00417 end += 4
00418 (length,) = _struct_I.unpack(str[start:end])
00419 pattern = '<%sd'%length
00420 start = end
00421 end += struct.calcsize(pattern)
00422 self.joint_stiffness = struct.unpack(pattern, str[start:end])
00423 start = end
00424 end += 4
00425 (length,) = _struct_I.unpack(str[start:end])
00426 pattern = '<%sd'%length
00427 start = end
00428 end += struct.calcsize(pattern)
00429 self.joint_damping = struct.unpack(pattern, str[start:end])
00430 _x = self
00431 start = end
00432 end += 112
00433 (_x.torso_pose.position.x, _x.torso_pose.position.y, _x.torso_pose.position.z, _x.torso_pose.orientation.x, _x.torso_pose.orientation.y, _x.torso_pose.orientation.z, _x.torso_pose.orientation.w, _x.hand_pose.position.x, _x.hand_pose.position.y, _x.hand_pose.position.z, _x.hand_pose.orientation.x, _x.hand_pose.orientation.y, _x.hand_pose.orientation.z, _x.hand_pose.orientation.w,) = _struct_14d.unpack(str[start:end])
00434 start = end
00435 end += 4
00436 (length,) = _struct_I.unpack(str[start:end])
00437 self.end_effector_jacobian.layout.dim = []
00438 for i in range(0, length):
00439 val1 = std_msgs.msg.MultiArrayDimension()
00440 start = end
00441 end += 4
00442 (length,) = _struct_I.unpack(str[start:end])
00443 start = end
00444 end += length
00445 if python3:
00446 val1.label = str[start:end].decode('utf-8')
00447 else:
00448 val1.label = str[start:end]
00449 _x = val1
00450 start = end
00451 end += 8
00452 (_x.size, _x.stride,) = _struct_2I.unpack(str[start:end])
00453 self.end_effector_jacobian.layout.dim.append(val1)
00454 start = end
00455 end += 4
00456 (self.end_effector_jacobian.layout.data_offset,) = _struct_I.unpack(str[start:end])
00457 start = end
00458 end += 4
00459 (length,) = _struct_I.unpack(str[start:end])
00460 pattern = '<%sd'%length
00461 start = end
00462 end += struct.calcsize(pattern)
00463 self.end_effector_jacobian.data = struct.unpack(pattern, str[start:end])
00464 start = end
00465 end += 4
00466 (length,) = _struct_I.unpack(str[start:end])
00467 self.contact_jacobians.layout.dim = []
00468 for i in range(0, length):
00469 val1 = std_msgs.msg.MultiArrayDimension()
00470 start = end
00471 end += 4
00472 (length,) = _struct_I.unpack(str[start:end])
00473 start = end
00474 end += length
00475 if python3:
00476 val1.label = str[start:end].decode('utf-8')
00477 else:
00478 val1.label = str[start:end]
00479 _x = val1
00480 start = end
00481 end += 8
00482 (_x.size, _x.stride,) = _struct_2I.unpack(str[start:end])
00483 self.contact_jacobians.layout.dim.append(val1)
00484 start = end
00485 end += 4
00486 (self.contact_jacobians.layout.data_offset,) = _struct_I.unpack(str[start:end])
00487 start = end
00488 end += 4
00489 (length,) = _struct_I.unpack(str[start:end])
00490 pattern = '<%sd'%length
00491 start = end
00492 end += struct.calcsize(pattern)
00493 self.contact_jacobians.data = struct.unpack(pattern, str[start:end])
00494 start = end
00495 end += 4
00496 (length,) = _struct_I.unpack(str[start:end])
00497 self.skins = []
00498 for i in range(0, length):
00499 val1 = hrl_haptic_manipulation_in_clutter_msgs.msg.TaxelArray()
00500 _v3 = val1.header
00501 start = end
00502 end += 4
00503 (_v3.seq,) = _struct_I.unpack(str[start:end])
00504 _v4 = _v3.stamp
00505 _x = _v4
00506 start = end
00507 end += 8
00508 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00509 start = end
00510 end += 4
00511 (length,) = _struct_I.unpack(str[start:end])
00512 start = end
00513 end += length
00514 if python3:
00515 _v3.frame_id = str[start:end].decode('utf-8')
00516 else:
00517 _v3.frame_id = str[start:end]
00518 start = end
00519 end += 4
00520 (length,) = _struct_I.unpack(str[start:end])
00521 start = end
00522 end += length
00523 if python3:
00524 val1.sensor_type = str[start:end].decode('utf-8')
00525 else:
00526 val1.sensor_type = str[start:end]
00527 start = end
00528 end += 4
00529 (length,) = _struct_I.unpack(str[start:end])
00530 val1.link_names = []
00531 for i in range(0, length):
00532 start = end
00533 end += 4
00534 (length,) = _struct_I.unpack(str[start:end])
00535 start = end
00536 end += length
00537 if python3:
00538 val2 = str[start:end].decode('utf-8')
00539 else:
00540 val2 = str[start:end]
00541 val1.link_names.append(val2)
00542 start = end
00543 end += 4
00544 (length,) = _struct_I.unpack(str[start:end])
00545 pattern = '<%sd'%length
00546 start = end
00547 end += struct.calcsize(pattern)
00548 val1.centers_x = struct.unpack(pattern, str[start:end])
00549 start = end
00550 end += 4
00551 (length,) = _struct_I.unpack(str[start:end])
00552 pattern = '<%sd'%length
00553 start = end
00554 end += struct.calcsize(pattern)
00555 val1.centers_y = struct.unpack(pattern, str[start:end])
00556 start = end
00557 end += 4
00558 (length,) = _struct_I.unpack(str[start:end])
00559 pattern = '<%sd'%length
00560 start = end
00561 end += struct.calcsize(pattern)
00562 val1.centers_z = struct.unpack(pattern, str[start:end])
00563 start = end
00564 end += 4
00565 (length,) = _struct_I.unpack(str[start:end])
00566 pattern = '<%sd'%length
00567 start = end
00568 end += struct.calcsize(pattern)
00569 val1.normals_x = struct.unpack(pattern, str[start:end])
00570 start = end
00571 end += 4
00572 (length,) = _struct_I.unpack(str[start:end])
00573 pattern = '<%sd'%length
00574 start = end
00575 end += struct.calcsize(pattern)
00576 val1.normals_y = struct.unpack(pattern, str[start:end])
00577 start = end
00578 end += 4
00579 (length,) = _struct_I.unpack(str[start:end])
00580 pattern = '<%sd'%length
00581 start = end
00582 end += struct.calcsize(pattern)
00583 val1.normals_z = struct.unpack(pattern, str[start:end])
00584 start = end
00585 end += 4
00586 (length,) = _struct_I.unpack(str[start:end])
00587 pattern = '<%sd'%length
00588 start = end
00589 end += struct.calcsize(pattern)
00590 val1.values_x = struct.unpack(pattern, str[start:end])
00591 start = end
00592 end += 4
00593 (length,) = _struct_I.unpack(str[start:end])
00594 pattern = '<%sd'%length
00595 start = end
00596 end += struct.calcsize(pattern)
00597 val1.values_y = struct.unpack(pattern, str[start:end])
00598 start = end
00599 end += 4
00600 (length,) = _struct_I.unpack(str[start:end])
00601 pattern = '<%sd'%length
00602 start = end
00603 end += struct.calcsize(pattern)
00604 val1.values_z = struct.unpack(pattern, str[start:end])
00605 self.skins.append(val1)
00606 return self
00607 except struct.error as e:
00608 raise genpy.DeserializationError(e)
00609
00610
00611 def serialize_numpy(self, buff, numpy):
00612 """
00613 serialize message with numpy array types into buffer
00614 :param buff: buffer, ``StringIO``
00615 :param numpy: numpy python module
00616 """
00617 try:
00618 _x = self
00619 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00620 _x = self.header.frame_id
00621 length = len(_x)
00622 if python3 or type(_x) == unicode:
00623 _x = _x.encode('utf-8')
00624 length = len(_x)
00625 buff.write(struct.pack('<I%ss'%length, length, _x))
00626 length = len(self.joint_names)
00627 buff.write(_struct_I.pack(length))
00628 for val1 in self.joint_names:
00629 length = len(val1)
00630 if python3 or type(val1) == unicode:
00631 val1 = val1.encode('utf-8')
00632 length = len(val1)
00633 buff.write(struct.pack('<I%ss'%length, length, val1))
00634 length = len(self.joint_angles)
00635 buff.write(_struct_I.pack(length))
00636 pattern = '<%sd'%length
00637 buff.write(self.joint_angles.tostring())
00638 length = len(self.desired_joint_angles)
00639 buff.write(_struct_I.pack(length))
00640 pattern = '<%sd'%length
00641 buff.write(self.desired_joint_angles.tostring())
00642 length = len(self.joint_velocities)
00643 buff.write(_struct_I.pack(length))
00644 pattern = '<%sd'%length
00645 buff.write(self.joint_velocities.tostring())
00646 length = len(self.joint_stiffness)
00647 buff.write(_struct_I.pack(length))
00648 pattern = '<%sd'%length
00649 buff.write(self.joint_stiffness.tostring())
00650 length = len(self.joint_damping)
00651 buff.write(_struct_I.pack(length))
00652 pattern = '<%sd'%length
00653 buff.write(self.joint_damping.tostring())
00654 _x = self
00655 buff.write(_struct_14d.pack(_x.torso_pose.position.x, _x.torso_pose.position.y, _x.torso_pose.position.z, _x.torso_pose.orientation.x, _x.torso_pose.orientation.y, _x.torso_pose.orientation.z, _x.torso_pose.orientation.w, _x.hand_pose.position.x, _x.hand_pose.position.y, _x.hand_pose.position.z, _x.hand_pose.orientation.x, _x.hand_pose.orientation.y, _x.hand_pose.orientation.z, _x.hand_pose.orientation.w))
00656 length = len(self.end_effector_jacobian.layout.dim)
00657 buff.write(_struct_I.pack(length))
00658 for val1 in self.end_effector_jacobian.layout.dim:
00659 _x = val1.label
00660 length = len(_x)
00661 if python3 or type(_x) == unicode:
00662 _x = _x.encode('utf-8')
00663 length = len(_x)
00664 buff.write(struct.pack('<I%ss'%length, length, _x))
00665 _x = val1
00666 buff.write(_struct_2I.pack(_x.size, _x.stride))
00667 buff.write(_struct_I.pack(self.end_effector_jacobian.layout.data_offset))
00668 length = len(self.end_effector_jacobian.data)
00669 buff.write(_struct_I.pack(length))
00670 pattern = '<%sd'%length
00671 buff.write(self.end_effector_jacobian.data.tostring())
00672 length = len(self.contact_jacobians.layout.dim)
00673 buff.write(_struct_I.pack(length))
00674 for val1 in self.contact_jacobians.layout.dim:
00675 _x = val1.label
00676 length = len(_x)
00677 if python3 or type(_x) == unicode:
00678 _x = _x.encode('utf-8')
00679 length = len(_x)
00680 buff.write(struct.pack('<I%ss'%length, length, _x))
00681 _x = val1
00682 buff.write(_struct_2I.pack(_x.size, _x.stride))
00683 buff.write(_struct_I.pack(self.contact_jacobians.layout.data_offset))
00684 length = len(self.contact_jacobians.data)
00685 buff.write(_struct_I.pack(length))
00686 pattern = '<%sd'%length
00687 buff.write(self.contact_jacobians.data.tostring())
00688 length = len(self.skins)
00689 buff.write(_struct_I.pack(length))
00690 for val1 in self.skins:
00691 _v5 = val1.header
00692 buff.write(_struct_I.pack(_v5.seq))
00693 _v6 = _v5.stamp
00694 _x = _v6
00695 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00696 _x = _v5.frame_id
00697 length = len(_x)
00698 if python3 or type(_x) == unicode:
00699 _x = _x.encode('utf-8')
00700 length = len(_x)
00701 buff.write(struct.pack('<I%ss'%length, length, _x))
00702 _x = val1.sensor_type
00703 length = len(_x)
00704 if python3 or type(_x) == unicode:
00705 _x = _x.encode('utf-8')
00706 length = len(_x)
00707 buff.write(struct.pack('<I%ss'%length, length, _x))
00708 length = len(val1.link_names)
00709 buff.write(_struct_I.pack(length))
00710 for val2 in val1.link_names:
00711 length = len(val2)
00712 if python3 or type(val2) == unicode:
00713 val2 = val2.encode('utf-8')
00714 length = len(val2)
00715 buff.write(struct.pack('<I%ss'%length, length, val2))
00716 length = len(val1.centers_x)
00717 buff.write(_struct_I.pack(length))
00718 pattern = '<%sd'%length
00719 buff.write(val1.centers_x.tostring())
00720 length = len(val1.centers_y)
00721 buff.write(_struct_I.pack(length))
00722 pattern = '<%sd'%length
00723 buff.write(val1.centers_y.tostring())
00724 length = len(val1.centers_z)
00725 buff.write(_struct_I.pack(length))
00726 pattern = '<%sd'%length
00727 buff.write(val1.centers_z.tostring())
00728 length = len(val1.normals_x)
00729 buff.write(_struct_I.pack(length))
00730 pattern = '<%sd'%length
00731 buff.write(val1.normals_x.tostring())
00732 length = len(val1.normals_y)
00733 buff.write(_struct_I.pack(length))
00734 pattern = '<%sd'%length
00735 buff.write(val1.normals_y.tostring())
00736 length = len(val1.normals_z)
00737 buff.write(_struct_I.pack(length))
00738 pattern = '<%sd'%length
00739 buff.write(val1.normals_z.tostring())
00740 length = len(val1.values_x)
00741 buff.write(_struct_I.pack(length))
00742 pattern = '<%sd'%length
00743 buff.write(val1.values_x.tostring())
00744 length = len(val1.values_y)
00745 buff.write(_struct_I.pack(length))
00746 pattern = '<%sd'%length
00747 buff.write(val1.values_y.tostring())
00748 length = len(val1.values_z)
00749 buff.write(_struct_I.pack(length))
00750 pattern = '<%sd'%length
00751 buff.write(val1.values_z.tostring())
00752 except struct.error as se: self._check_types(se)
00753 except TypeError as te: self._check_types(te)
00754
00755 def deserialize_numpy(self, str, numpy):
00756 """
00757 unpack serialized message in str into this message instance using numpy for array types
00758 :param str: byte array of serialized message, ``str``
00759 :param numpy: numpy python module
00760 """
00761 try:
00762 if self.header is None:
00763 self.header = std_msgs.msg.Header()
00764 if self.torso_pose is None:
00765 self.torso_pose = geometry_msgs.msg.Pose()
00766 if self.hand_pose is None:
00767 self.hand_pose = geometry_msgs.msg.Pose()
00768 if self.end_effector_jacobian is None:
00769 self.end_effector_jacobian = std_msgs.msg.Float64MultiArray()
00770 if self.contact_jacobians is None:
00771 self.contact_jacobians = std_msgs.msg.Float64MultiArray()
00772 if self.skins is None:
00773 self.skins = None
00774 end = 0
00775 _x = self
00776 start = end
00777 end += 12
00778 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00779 start = end
00780 end += 4
00781 (length,) = _struct_I.unpack(str[start:end])
00782 start = end
00783 end += length
00784 if python3:
00785 self.header.frame_id = str[start:end].decode('utf-8')
00786 else:
00787 self.header.frame_id = str[start:end]
00788 start = end
00789 end += 4
00790 (length,) = _struct_I.unpack(str[start:end])
00791 self.joint_names = []
00792 for i in range(0, length):
00793 start = end
00794 end += 4
00795 (length,) = _struct_I.unpack(str[start:end])
00796 start = end
00797 end += length
00798 if python3:
00799 val1 = str[start:end].decode('utf-8')
00800 else:
00801 val1 = str[start:end]
00802 self.joint_names.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.joint_angles = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
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.desired_joint_angles = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
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.joint_velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00824 start = end
00825 end += 4
00826 (length,) = _struct_I.unpack(str[start:end])
00827 pattern = '<%sd'%length
00828 start = end
00829 end += struct.calcsize(pattern)
00830 self.joint_stiffness = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00831 start = end
00832 end += 4
00833 (length,) = _struct_I.unpack(str[start:end])
00834 pattern = '<%sd'%length
00835 start = end
00836 end += struct.calcsize(pattern)
00837 self.joint_damping = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00838 _x = self
00839 start = end
00840 end += 112
00841 (_x.torso_pose.position.x, _x.torso_pose.position.y, _x.torso_pose.position.z, _x.torso_pose.orientation.x, _x.torso_pose.orientation.y, _x.torso_pose.orientation.z, _x.torso_pose.orientation.w, _x.hand_pose.position.x, _x.hand_pose.position.y, _x.hand_pose.position.z, _x.hand_pose.orientation.x, _x.hand_pose.orientation.y, _x.hand_pose.orientation.z, _x.hand_pose.orientation.w,) = _struct_14d.unpack(str[start:end])
00842 start = end
00843 end += 4
00844 (length,) = _struct_I.unpack(str[start:end])
00845 self.end_effector_jacobian.layout.dim = []
00846 for i in range(0, length):
00847 val1 = std_msgs.msg.MultiArrayDimension()
00848 start = end
00849 end += 4
00850 (length,) = _struct_I.unpack(str[start:end])
00851 start = end
00852 end += length
00853 if python3:
00854 val1.label = str[start:end].decode('utf-8')
00855 else:
00856 val1.label = str[start:end]
00857 _x = val1
00858 start = end
00859 end += 8
00860 (_x.size, _x.stride,) = _struct_2I.unpack(str[start:end])
00861 self.end_effector_jacobian.layout.dim.append(val1)
00862 start = end
00863 end += 4
00864 (self.end_effector_jacobian.layout.data_offset,) = _struct_I.unpack(str[start:end])
00865 start = end
00866 end += 4
00867 (length,) = _struct_I.unpack(str[start:end])
00868 pattern = '<%sd'%length
00869 start = end
00870 end += struct.calcsize(pattern)
00871 self.end_effector_jacobian.data = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00872 start = end
00873 end += 4
00874 (length,) = _struct_I.unpack(str[start:end])
00875 self.contact_jacobians.layout.dim = []
00876 for i in range(0, length):
00877 val1 = std_msgs.msg.MultiArrayDimension()
00878 start = end
00879 end += 4
00880 (length,) = _struct_I.unpack(str[start:end])
00881 start = end
00882 end += length
00883 if python3:
00884 val1.label = str[start:end].decode('utf-8')
00885 else:
00886 val1.label = str[start:end]
00887 _x = val1
00888 start = end
00889 end += 8
00890 (_x.size, _x.stride,) = _struct_2I.unpack(str[start:end])
00891 self.contact_jacobians.layout.dim.append(val1)
00892 start = end
00893 end += 4
00894 (self.contact_jacobians.layout.data_offset,) = _struct_I.unpack(str[start:end])
00895 start = end
00896 end += 4
00897 (length,) = _struct_I.unpack(str[start:end])
00898 pattern = '<%sd'%length
00899 start = end
00900 end += struct.calcsize(pattern)
00901 self.contact_jacobians.data = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00902 start = end
00903 end += 4
00904 (length,) = _struct_I.unpack(str[start:end])
00905 self.skins = []
00906 for i in range(0, length):
00907 val1 = hrl_haptic_manipulation_in_clutter_msgs.msg.TaxelArray()
00908 _v7 = val1.header
00909 start = end
00910 end += 4
00911 (_v7.seq,) = _struct_I.unpack(str[start:end])
00912 _v8 = _v7.stamp
00913 _x = _v8
00914 start = end
00915 end += 8
00916 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00917 start = end
00918 end += 4
00919 (length,) = _struct_I.unpack(str[start:end])
00920 start = end
00921 end += length
00922 if python3:
00923 _v7.frame_id = str[start:end].decode('utf-8')
00924 else:
00925 _v7.frame_id = str[start:end]
00926 start = end
00927 end += 4
00928 (length,) = _struct_I.unpack(str[start:end])
00929 start = end
00930 end += length
00931 if python3:
00932 val1.sensor_type = str[start:end].decode('utf-8')
00933 else:
00934 val1.sensor_type = str[start:end]
00935 start = end
00936 end += 4
00937 (length,) = _struct_I.unpack(str[start:end])
00938 val1.link_names = []
00939 for i in range(0, length):
00940 start = end
00941 end += 4
00942 (length,) = _struct_I.unpack(str[start:end])
00943 start = end
00944 end += length
00945 if python3:
00946 val2 = str[start:end].decode('utf-8')
00947 else:
00948 val2 = str[start:end]
00949 val1.link_names.append(val2)
00950 start = end
00951 end += 4
00952 (length,) = _struct_I.unpack(str[start:end])
00953 pattern = '<%sd'%length
00954 start = end
00955 end += struct.calcsize(pattern)
00956 val1.centers_x = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00957 start = end
00958 end += 4
00959 (length,) = _struct_I.unpack(str[start:end])
00960 pattern = '<%sd'%length
00961 start = end
00962 end += struct.calcsize(pattern)
00963 val1.centers_y = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00964 start = end
00965 end += 4
00966 (length,) = _struct_I.unpack(str[start:end])
00967 pattern = '<%sd'%length
00968 start = end
00969 end += struct.calcsize(pattern)
00970 val1.centers_z = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00971 start = end
00972 end += 4
00973 (length,) = _struct_I.unpack(str[start:end])
00974 pattern = '<%sd'%length
00975 start = end
00976 end += struct.calcsize(pattern)
00977 val1.normals_x = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00978 start = end
00979 end += 4
00980 (length,) = _struct_I.unpack(str[start:end])
00981 pattern = '<%sd'%length
00982 start = end
00983 end += struct.calcsize(pattern)
00984 val1.normals_y = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00985 start = end
00986 end += 4
00987 (length,) = _struct_I.unpack(str[start:end])
00988 pattern = '<%sd'%length
00989 start = end
00990 end += struct.calcsize(pattern)
00991 val1.normals_z = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00992 start = end
00993 end += 4
00994 (length,) = _struct_I.unpack(str[start:end])
00995 pattern = '<%sd'%length
00996 start = end
00997 end += struct.calcsize(pattern)
00998 val1.values_x = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00999 start = end
01000 end += 4
01001 (length,) = _struct_I.unpack(str[start:end])
01002 pattern = '<%sd'%length
01003 start = end
01004 end += struct.calcsize(pattern)
01005 val1.values_y = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01006 start = end
01007 end += 4
01008 (length,) = _struct_I.unpack(str[start:end])
01009 pattern = '<%sd'%length
01010 start = end
01011 end += struct.calcsize(pattern)
01012 val1.values_z = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01013 self.skins.append(val1)
01014 return self
01015 except struct.error as e:
01016 raise genpy.DeserializationError(e)
01017
01018 _struct_I = genpy.struct_I
01019 _struct_3I = struct.Struct("<3I")
01020 _struct_14d = struct.Struct("<14d")
01021 _struct_2I = struct.Struct("<2I")