_GraspHandPostureExecutionGoal.py
Go to the documentation of this file.
00001 """autogenerated by genpy from object_manipulation_msgs/GraspHandPostureExecutionGoal.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 manipulation_msgs.msg
00009 import std_msgs.msg
00010 import sensor_msgs.msg
00011 
00012 class GraspHandPostureExecutionGoal(genpy.Message):
00013   _md5sum = "d84b52613849c9277c855c36241b2a5d"
00014   _type = "object_manipulation_msgs/GraspHandPostureExecutionGoal"
00015   _has_header = False #flag to mark the presence of a Header object
00016   _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00017 # an action for requesting the finger posture part of grasp be physically carried out by a hand
00018 # the name of the arm being used is not in here, as this will be sent to a specific action server
00019 # for each arm
00020 
00021 # the grasp to be executed
00022 manipulation_msgs/Grasp grasp
00023 
00024 # the goal of this action
00025 # requests that the hand be set in the pre-grasp posture
00026 int32 PRE_GRASP=1
00027 # requests that the hand execute the actual grasp
00028 int32 GRASP=2
00029 # requests that the hand open to release the object
00030 int32 RELEASE=3
00031 int32 goal
00032 
00033 # the max contact force to use (<=0 if no desired max)
00034 float32 max_contact_force
00035 
00036 
00037 ================================================================================
00038 MSG: manipulation_msgs/Grasp
00039 # A name for this grasp
00040 string id
00041 
00042 # The internal posture of the hand for the pre-grasp
00043 # only positions are used
00044 sensor_msgs/JointState pre_grasp_posture
00045 
00046 # The internal posture of the hand for the grasp
00047 # positions and efforts are used
00048 sensor_msgs/JointState grasp_posture
00049 
00050 # The position of the end-effector for the grasp relative to a reference frame 
00051 # (that is always specified elsewhere, not in this message)
00052 geometry_msgs/PoseStamped grasp_pose
00053 
00054 # The estimated probability of success for this grasp, or some other
00055 # measure of how "good" it is.
00056 float64 grasp_quality
00057 
00058 # The approach motion
00059 GripperTranslation approach
00060 
00061 # The retreat motion
00062 GripperTranslation retreat
00063 
00064 # the maximum contact force to use while grasping (<=0 to disable)
00065 float32 max_contact_force
00066 
00067 # an optional list of obstacles that we have semantic information about
00068 # and that can be touched/pushed/moved in the course of grasping
00069 string[] allowed_touch_objects
00070 
00071 ================================================================================
00072 MSG: sensor_msgs/JointState
00073 # This is a message that holds data to describe the state of a set of torque controlled joints. 
00074 #
00075 # The state of each joint (revolute or prismatic) is defined by:
00076 #  * the position of the joint (rad or m),
00077 #  * the velocity of the joint (rad/s or m/s) and 
00078 #  * the effort that is applied in the joint (Nm or N).
00079 #
00080 # Each joint is uniquely identified by its name
00081 # The header specifies the time at which the joint states were recorded. All the joint states
00082 # in one message have to be recorded at the same time.
00083 #
00084 # This message consists of a multiple arrays, one for each part of the joint state. 
00085 # The goal is to make each of the fields optional. When e.g. your joints have no
00086 # effort associated with them, you can leave the effort array empty. 
00087 #
00088 # All arrays in this message should have the same size, or be empty.
00089 # This is the only way to uniquely associate the joint name with the correct
00090 # states.
00091 
00092 
00093 Header header
00094 
00095 string[] name
00096 float64[] position
00097 float64[] velocity
00098 float64[] effort
00099 
00100 ================================================================================
00101 MSG: std_msgs/Header
00102 # Standard metadata for higher-level stamped data types.
00103 # This is generally used to communicate timestamped data 
00104 # in a particular coordinate frame.
00105 # 
00106 # sequence ID: consecutively increasing ID 
00107 uint32 seq
00108 #Two-integer timestamp that is expressed as:
00109 # * stamp.secs: seconds (stamp_secs) since epoch
00110 # * stamp.nsecs: nanoseconds since stamp_secs
00111 # time-handling sugar is provided by the client library
00112 time stamp
00113 #Frame this data is associated with
00114 # 0: no frame
00115 # 1: global frame
00116 string frame_id
00117 
00118 ================================================================================
00119 MSG: geometry_msgs/PoseStamped
00120 # A Pose with reference coordinate frame and timestamp
00121 Header header
00122 Pose pose
00123 
00124 ================================================================================
00125 MSG: geometry_msgs/Pose
00126 # A representation of pose in free space, composed of postion and orientation. 
00127 Point position
00128 Quaternion orientation
00129 
00130 ================================================================================
00131 MSG: geometry_msgs/Point
00132 # This contains the position of a point in free space
00133 float64 x
00134 float64 y
00135 float64 z
00136 
00137 ================================================================================
00138 MSG: geometry_msgs/Quaternion
00139 # This represents an orientation in free space in quaternion form.
00140 
00141 float64 x
00142 float64 y
00143 float64 z
00144 float64 w
00145 
00146 ================================================================================
00147 MSG: manipulation_msgs/GripperTranslation
00148 # defines a translation for the gripper, used in pickup or place tasks
00149 # for example for lifting an object off a table or approaching the table for placing
00150 
00151 # the direction of the translation
00152 geometry_msgs/Vector3Stamped direction
00153 
00154 # the desired translation distance
00155 float32 desired_distance
00156 
00157 # the min distance that must be considered feasible before the
00158 # grasp is even attempted
00159 float32 min_distance
00160 
00161 ================================================================================
00162 MSG: geometry_msgs/Vector3Stamped
00163 # This represents a Vector3 with reference coordinate frame and timestamp
00164 Header header
00165 Vector3 vector
00166 
00167 ================================================================================
00168 MSG: geometry_msgs/Vector3
00169 # This represents a vector in free space. 
00170 
00171 float64 x
00172 float64 y
00173 float64 z
00174 """
00175   # Pseudo-constants
00176   PRE_GRASP = 1
00177   GRASP = 2
00178   RELEASE = 3
00179 
00180   __slots__ = ['grasp','goal','max_contact_force']
00181   _slot_types = ['manipulation_msgs/Grasp','int32','float32']
00182 
00183   def __init__(self, *args, **kwds):
00184     """
00185     Constructor. Any message fields that are implicitly/explicitly
00186     set to None will be assigned a default value. The recommend
00187     use is keyword arguments as this is more robust to future message
00188     changes.  You cannot mix in-order arguments and keyword arguments.
00189 
00190     The available fields are:
00191        grasp,goal,max_contact_force
00192 
00193     :param args: complete set of field values, in .msg order
00194     :param kwds: use keyword arguments corresponding to message field names
00195     to set specific fields.
00196     """
00197     if args or kwds:
00198       super(GraspHandPostureExecutionGoal, self).__init__(*args, **kwds)
00199       #message fields cannot be None, assign default values for those that are
00200       if self.grasp is None:
00201         self.grasp = manipulation_msgs.msg.Grasp()
00202       if self.goal is None:
00203         self.goal = 0
00204       if self.max_contact_force is None:
00205         self.max_contact_force = 0.
00206     else:
00207       self.grasp = manipulation_msgs.msg.Grasp()
00208       self.goal = 0
00209       self.max_contact_force = 0.
00210 
00211   def _get_types(self):
00212     """
00213     internal API method
00214     """
00215     return self._slot_types
00216 
00217   def serialize(self, buff):
00218     """
00219     serialize message into buffer
00220     :param buff: buffer, ``StringIO``
00221     """
00222     try:
00223       _x = self.grasp.id
00224       length = len(_x)
00225       if python3 or type(_x) == unicode:
00226         _x = _x.encode('utf-8')
00227         length = len(_x)
00228       buff.write(struct.pack('<I%ss'%length, length, _x))
00229       _x = self
00230       buff.write(_struct_3I.pack(_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
00231       _x = self.grasp.pre_grasp_posture.header.frame_id
00232       length = len(_x)
00233       if python3 or type(_x) == unicode:
00234         _x = _x.encode('utf-8')
00235         length = len(_x)
00236       buff.write(struct.pack('<I%ss'%length, length, _x))
00237       length = len(self.grasp.pre_grasp_posture.name)
00238       buff.write(_struct_I.pack(length))
00239       for val1 in self.grasp.pre_grasp_posture.name:
00240         length = len(val1)
00241         if python3 or type(val1) == unicode:
00242           val1 = val1.encode('utf-8')
00243           length = len(val1)
00244         buff.write(struct.pack('<I%ss'%length, length, val1))
00245       length = len(self.grasp.pre_grasp_posture.position)
00246       buff.write(_struct_I.pack(length))
00247       pattern = '<%sd'%length
00248       buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.position))
00249       length = len(self.grasp.pre_grasp_posture.velocity)
00250       buff.write(_struct_I.pack(length))
00251       pattern = '<%sd'%length
00252       buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.velocity))
00253       length = len(self.grasp.pre_grasp_posture.effort)
00254       buff.write(_struct_I.pack(length))
00255       pattern = '<%sd'%length
00256       buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.effort))
00257       _x = self
00258       buff.write(_struct_3I.pack(_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs))
00259       _x = self.grasp.grasp_posture.header.frame_id
00260       length = len(_x)
00261       if python3 or type(_x) == unicode:
00262         _x = _x.encode('utf-8')
00263         length = len(_x)
00264       buff.write(struct.pack('<I%ss'%length, length, _x))
00265       length = len(self.grasp.grasp_posture.name)
00266       buff.write(_struct_I.pack(length))
00267       for val1 in self.grasp.grasp_posture.name:
00268         length = len(val1)
00269         if python3 or type(val1) == unicode:
00270           val1 = val1.encode('utf-8')
00271           length = len(val1)
00272         buff.write(struct.pack('<I%ss'%length, length, val1))
00273       length = len(self.grasp.grasp_posture.position)
00274       buff.write(_struct_I.pack(length))
00275       pattern = '<%sd'%length
00276       buff.write(struct.pack(pattern, *self.grasp.grasp_posture.position))
00277       length = len(self.grasp.grasp_posture.velocity)
00278       buff.write(_struct_I.pack(length))
00279       pattern = '<%sd'%length
00280       buff.write(struct.pack(pattern, *self.grasp.grasp_posture.velocity))
00281       length = len(self.grasp.grasp_posture.effort)
00282       buff.write(_struct_I.pack(length))
00283       pattern = '<%sd'%length
00284       buff.write(struct.pack(pattern, *self.grasp.grasp_posture.effort))
00285       _x = self
00286       buff.write(_struct_3I.pack(_x.grasp.grasp_pose.header.seq, _x.grasp.grasp_pose.header.stamp.secs, _x.grasp.grasp_pose.header.stamp.nsecs))
00287       _x = self.grasp.grasp_pose.header.frame_id
00288       length = len(_x)
00289       if python3 or type(_x) == unicode:
00290         _x = _x.encode('utf-8')
00291         length = len(_x)
00292       buff.write(struct.pack('<I%ss'%length, length, _x))
00293       _x = self
00294       buff.write(_struct_8d3I.pack(_x.grasp.grasp_pose.pose.position.x, _x.grasp.grasp_pose.pose.position.y, _x.grasp.grasp_pose.pose.position.z, _x.grasp.grasp_pose.pose.orientation.x, _x.grasp.grasp_pose.pose.orientation.y, _x.grasp.grasp_pose.pose.orientation.z, _x.grasp.grasp_pose.pose.orientation.w, _x.grasp.grasp_quality, _x.grasp.approach.direction.header.seq, _x.grasp.approach.direction.header.stamp.secs, _x.grasp.approach.direction.header.stamp.nsecs))
00295       _x = self.grasp.approach.direction.header.frame_id
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       _x = self
00302       buff.write(_struct_3d2f3I.pack(_x.grasp.approach.direction.vector.x, _x.grasp.approach.direction.vector.y, _x.grasp.approach.direction.vector.z, _x.grasp.approach.desired_distance, _x.grasp.approach.min_distance, _x.grasp.retreat.direction.header.seq, _x.grasp.retreat.direction.header.stamp.secs, _x.grasp.retreat.direction.header.stamp.nsecs))
00303       _x = self.grasp.retreat.direction.header.frame_id
00304       length = len(_x)
00305       if python3 or type(_x) == unicode:
00306         _x = _x.encode('utf-8')
00307         length = len(_x)
00308       buff.write(struct.pack('<I%ss'%length, length, _x))
00309       _x = self
00310       buff.write(_struct_3d3f.pack(_x.grasp.retreat.direction.vector.x, _x.grasp.retreat.direction.vector.y, _x.grasp.retreat.direction.vector.z, _x.grasp.retreat.desired_distance, _x.grasp.retreat.min_distance, _x.grasp.max_contact_force))
00311       length = len(self.grasp.allowed_touch_objects)
00312       buff.write(_struct_I.pack(length))
00313       for val1 in self.grasp.allowed_touch_objects:
00314         length = len(val1)
00315         if python3 or type(val1) == unicode:
00316           val1 = val1.encode('utf-8')
00317           length = len(val1)
00318         buff.write(struct.pack('<I%ss'%length, length, val1))
00319       _x = self
00320       buff.write(_struct_if.pack(_x.goal, _x.max_contact_force))
00321     except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00322     except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00323 
00324   def deserialize(self, str):
00325     """
00326     unpack serialized message in str into this message instance
00327     :param str: byte array of serialized message, ``str``
00328     """
00329     try:
00330       if self.grasp is None:
00331         self.grasp = manipulation_msgs.msg.Grasp()
00332       end = 0
00333       start = end
00334       end += 4
00335       (length,) = _struct_I.unpack(str[start:end])
00336       start = end
00337       end += length
00338       if python3:
00339         self.grasp.id = str[start:end].decode('utf-8')
00340       else:
00341         self.grasp.id = str[start:end]
00342       _x = self
00343       start = end
00344       end += 12
00345       (_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00346       start = end
00347       end += 4
00348       (length,) = _struct_I.unpack(str[start:end])
00349       start = end
00350       end += length
00351       if python3:
00352         self.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
00353       else:
00354         self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
00355       start = end
00356       end += 4
00357       (length,) = _struct_I.unpack(str[start:end])
00358       self.grasp.pre_grasp_posture.name = []
00359       for i in range(0, length):
00360         start = end
00361         end += 4
00362         (length,) = _struct_I.unpack(str[start:end])
00363         start = end
00364         end += length
00365         if python3:
00366           val1 = str[start:end].decode('utf-8')
00367         else:
00368           val1 = str[start:end]
00369         self.grasp.pre_grasp_posture.name.append(val1)
00370       start = end
00371       end += 4
00372       (length,) = _struct_I.unpack(str[start:end])
00373       pattern = '<%sd'%length
00374       start = end
00375       end += struct.calcsize(pattern)
00376       self.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
00377       start = end
00378       end += 4
00379       (length,) = _struct_I.unpack(str[start:end])
00380       pattern = '<%sd'%length
00381       start = end
00382       end += struct.calcsize(pattern)
00383       self.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00384       start = end
00385       end += 4
00386       (length,) = _struct_I.unpack(str[start:end])
00387       pattern = '<%sd'%length
00388       start = end
00389       end += struct.calcsize(pattern)
00390       self.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
00391       _x = self
00392       start = end
00393       end += 12
00394       (_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00395       start = end
00396       end += 4
00397       (length,) = _struct_I.unpack(str[start:end])
00398       start = end
00399       end += length
00400       if python3:
00401         self.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
00402       else:
00403         self.grasp.grasp_posture.header.frame_id = str[start:end]
00404       start = end
00405       end += 4
00406       (length,) = _struct_I.unpack(str[start:end])
00407       self.grasp.grasp_posture.name = []
00408       for i in range(0, length):
00409         start = end
00410         end += 4
00411         (length,) = _struct_I.unpack(str[start:end])
00412         start = end
00413         end += length
00414         if python3:
00415           val1 = str[start:end].decode('utf-8')
00416         else:
00417           val1 = str[start:end]
00418         self.grasp.grasp_posture.name.append(val1)
00419       start = end
00420       end += 4
00421       (length,) = _struct_I.unpack(str[start:end])
00422       pattern = '<%sd'%length
00423       start = end
00424       end += struct.calcsize(pattern)
00425       self.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end])
00426       start = end
00427       end += 4
00428       (length,) = _struct_I.unpack(str[start:end])
00429       pattern = '<%sd'%length
00430       start = end
00431       end += struct.calcsize(pattern)
00432       self.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00433       start = end
00434       end += 4
00435       (length,) = _struct_I.unpack(str[start:end])
00436       pattern = '<%sd'%length
00437       start = end
00438       end += struct.calcsize(pattern)
00439       self.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end])
00440       _x = self
00441       start = end
00442       end += 12
00443       (_x.grasp.grasp_pose.header.seq, _x.grasp.grasp_pose.header.stamp.secs, _x.grasp.grasp_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00444       start = end
00445       end += 4
00446       (length,) = _struct_I.unpack(str[start:end])
00447       start = end
00448       end += length
00449       if python3:
00450         self.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
00451       else:
00452         self.grasp.grasp_pose.header.frame_id = str[start:end]
00453       _x = self
00454       start = end
00455       end += 76
00456       (_x.grasp.grasp_pose.pose.position.x, _x.grasp.grasp_pose.pose.position.y, _x.grasp.grasp_pose.pose.position.z, _x.grasp.grasp_pose.pose.orientation.x, _x.grasp.grasp_pose.pose.orientation.y, _x.grasp.grasp_pose.pose.orientation.z, _x.grasp.grasp_pose.pose.orientation.w, _x.grasp.grasp_quality, _x.grasp.approach.direction.header.seq, _x.grasp.approach.direction.header.stamp.secs, _x.grasp.approach.direction.header.stamp.nsecs,) = _struct_8d3I.unpack(str[start:end])
00457       start = end
00458       end += 4
00459       (length,) = _struct_I.unpack(str[start:end])
00460       start = end
00461       end += length
00462       if python3:
00463         self.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
00464       else:
00465         self.grasp.approach.direction.header.frame_id = str[start:end]
00466       _x = self
00467       start = end
00468       end += 44
00469       (_x.grasp.approach.direction.vector.x, _x.grasp.approach.direction.vector.y, _x.grasp.approach.direction.vector.z, _x.grasp.approach.desired_distance, _x.grasp.approach.min_distance, _x.grasp.retreat.direction.header.seq, _x.grasp.retreat.direction.header.stamp.secs, _x.grasp.retreat.direction.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
00470       start = end
00471       end += 4
00472       (length,) = _struct_I.unpack(str[start:end])
00473       start = end
00474       end += length
00475       if python3:
00476         self.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
00477       else:
00478         self.grasp.retreat.direction.header.frame_id = str[start:end]
00479       _x = self
00480       start = end
00481       end += 36
00482       (_x.grasp.retreat.direction.vector.x, _x.grasp.retreat.direction.vector.y, _x.grasp.retreat.direction.vector.z, _x.grasp.retreat.desired_distance, _x.grasp.retreat.min_distance, _x.grasp.max_contact_force,) = _struct_3d3f.unpack(str[start:end])
00483       start = end
00484       end += 4
00485       (length,) = _struct_I.unpack(str[start:end])
00486       self.grasp.allowed_touch_objects = []
00487       for i in range(0, length):
00488         start = end
00489         end += 4
00490         (length,) = _struct_I.unpack(str[start:end])
00491         start = end
00492         end += length
00493         if python3:
00494           val1 = str[start:end].decode('utf-8')
00495         else:
00496           val1 = str[start:end]
00497         self.grasp.allowed_touch_objects.append(val1)
00498       _x = self
00499       start = end
00500       end += 8
00501       (_x.goal, _x.max_contact_force,) = _struct_if.unpack(str[start:end])
00502       return self
00503     except struct.error as e:
00504       raise genpy.DeserializationError(e) #most likely buffer underfill
00505 
00506 
00507   def serialize_numpy(self, buff, numpy):
00508     """
00509     serialize message with numpy array types into buffer
00510     :param buff: buffer, ``StringIO``
00511     :param numpy: numpy python module
00512     """
00513     try:
00514       _x = self.grasp.id
00515       length = len(_x)
00516       if python3 or type(_x) == unicode:
00517         _x = _x.encode('utf-8')
00518         length = len(_x)
00519       buff.write(struct.pack('<I%ss'%length, length, _x))
00520       _x = self
00521       buff.write(_struct_3I.pack(_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
00522       _x = self.grasp.pre_grasp_posture.header.frame_id
00523       length = len(_x)
00524       if python3 or type(_x) == unicode:
00525         _x = _x.encode('utf-8')
00526         length = len(_x)
00527       buff.write(struct.pack('<I%ss'%length, length, _x))
00528       length = len(self.grasp.pre_grasp_posture.name)
00529       buff.write(_struct_I.pack(length))
00530       for val1 in self.grasp.pre_grasp_posture.name:
00531         length = len(val1)
00532         if python3 or type(val1) == unicode:
00533           val1 = val1.encode('utf-8')
00534           length = len(val1)
00535         buff.write(struct.pack('<I%ss'%length, length, val1))
00536       length = len(self.grasp.pre_grasp_posture.position)
00537       buff.write(_struct_I.pack(length))
00538       pattern = '<%sd'%length
00539       buff.write(self.grasp.pre_grasp_posture.position.tostring())
00540       length = len(self.grasp.pre_grasp_posture.velocity)
00541       buff.write(_struct_I.pack(length))
00542       pattern = '<%sd'%length
00543       buff.write(self.grasp.pre_grasp_posture.velocity.tostring())
00544       length = len(self.grasp.pre_grasp_posture.effort)
00545       buff.write(_struct_I.pack(length))
00546       pattern = '<%sd'%length
00547       buff.write(self.grasp.pre_grasp_posture.effort.tostring())
00548       _x = self
00549       buff.write(_struct_3I.pack(_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs))
00550       _x = self.grasp.grasp_posture.header.frame_id
00551       length = len(_x)
00552       if python3 or type(_x) == unicode:
00553         _x = _x.encode('utf-8')
00554         length = len(_x)
00555       buff.write(struct.pack('<I%ss'%length, length, _x))
00556       length = len(self.grasp.grasp_posture.name)
00557       buff.write(_struct_I.pack(length))
00558       for val1 in self.grasp.grasp_posture.name:
00559         length = len(val1)
00560         if python3 or type(val1) == unicode:
00561           val1 = val1.encode('utf-8')
00562           length = len(val1)
00563         buff.write(struct.pack('<I%ss'%length, length, val1))
00564       length = len(self.grasp.grasp_posture.position)
00565       buff.write(_struct_I.pack(length))
00566       pattern = '<%sd'%length
00567       buff.write(self.grasp.grasp_posture.position.tostring())
00568       length = len(self.grasp.grasp_posture.velocity)
00569       buff.write(_struct_I.pack(length))
00570       pattern = '<%sd'%length
00571       buff.write(self.grasp.grasp_posture.velocity.tostring())
00572       length = len(self.grasp.grasp_posture.effort)
00573       buff.write(_struct_I.pack(length))
00574       pattern = '<%sd'%length
00575       buff.write(self.grasp.grasp_posture.effort.tostring())
00576       _x = self
00577       buff.write(_struct_3I.pack(_x.grasp.grasp_pose.header.seq, _x.grasp.grasp_pose.header.stamp.secs, _x.grasp.grasp_pose.header.stamp.nsecs))
00578       _x = self.grasp.grasp_pose.header.frame_id
00579       length = len(_x)
00580       if python3 or type(_x) == unicode:
00581         _x = _x.encode('utf-8')
00582         length = len(_x)
00583       buff.write(struct.pack('<I%ss'%length, length, _x))
00584       _x = self
00585       buff.write(_struct_8d3I.pack(_x.grasp.grasp_pose.pose.position.x, _x.grasp.grasp_pose.pose.position.y, _x.grasp.grasp_pose.pose.position.z, _x.grasp.grasp_pose.pose.orientation.x, _x.grasp.grasp_pose.pose.orientation.y, _x.grasp.grasp_pose.pose.orientation.z, _x.grasp.grasp_pose.pose.orientation.w, _x.grasp.grasp_quality, _x.grasp.approach.direction.header.seq, _x.grasp.approach.direction.header.stamp.secs, _x.grasp.approach.direction.header.stamp.nsecs))
00586       _x = self.grasp.approach.direction.header.frame_id
00587       length = len(_x)
00588       if python3 or type(_x) == unicode:
00589         _x = _x.encode('utf-8')
00590         length = len(_x)
00591       buff.write(struct.pack('<I%ss'%length, length, _x))
00592       _x = self
00593       buff.write(_struct_3d2f3I.pack(_x.grasp.approach.direction.vector.x, _x.grasp.approach.direction.vector.y, _x.grasp.approach.direction.vector.z, _x.grasp.approach.desired_distance, _x.grasp.approach.min_distance, _x.grasp.retreat.direction.header.seq, _x.grasp.retreat.direction.header.stamp.secs, _x.grasp.retreat.direction.header.stamp.nsecs))
00594       _x = self.grasp.retreat.direction.header.frame_id
00595       length = len(_x)
00596       if python3 or type(_x) == unicode:
00597         _x = _x.encode('utf-8')
00598         length = len(_x)
00599       buff.write(struct.pack('<I%ss'%length, length, _x))
00600       _x = self
00601       buff.write(_struct_3d3f.pack(_x.grasp.retreat.direction.vector.x, _x.grasp.retreat.direction.vector.y, _x.grasp.retreat.direction.vector.z, _x.grasp.retreat.desired_distance, _x.grasp.retreat.min_distance, _x.grasp.max_contact_force))
00602       length = len(self.grasp.allowed_touch_objects)
00603       buff.write(_struct_I.pack(length))
00604       for val1 in self.grasp.allowed_touch_objects:
00605         length = len(val1)
00606         if python3 or type(val1) == unicode:
00607           val1 = val1.encode('utf-8')
00608           length = len(val1)
00609         buff.write(struct.pack('<I%ss'%length, length, val1))
00610       _x = self
00611       buff.write(_struct_if.pack(_x.goal, _x.max_contact_force))
00612     except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00613     except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00614 
00615   def deserialize_numpy(self, str, numpy):
00616     """
00617     unpack serialized message in str into this message instance using numpy for array types
00618     :param str: byte array of serialized message, ``str``
00619     :param numpy: numpy python module
00620     """
00621     try:
00622       if self.grasp is None:
00623         self.grasp = manipulation_msgs.msg.Grasp()
00624       end = 0
00625       start = end
00626       end += 4
00627       (length,) = _struct_I.unpack(str[start:end])
00628       start = end
00629       end += length
00630       if python3:
00631         self.grasp.id = str[start:end].decode('utf-8')
00632       else:
00633         self.grasp.id = str[start:end]
00634       _x = self
00635       start = end
00636       end += 12
00637       (_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00638       start = end
00639       end += 4
00640       (length,) = _struct_I.unpack(str[start:end])
00641       start = end
00642       end += length
00643       if python3:
00644         self.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
00645       else:
00646         self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
00647       start = end
00648       end += 4
00649       (length,) = _struct_I.unpack(str[start:end])
00650       self.grasp.pre_grasp_posture.name = []
00651       for i in range(0, length):
00652         start = end
00653         end += 4
00654         (length,) = _struct_I.unpack(str[start:end])
00655         start = end
00656         end += length
00657         if python3:
00658           val1 = str[start:end].decode('utf-8')
00659         else:
00660           val1 = str[start:end]
00661         self.grasp.pre_grasp_posture.name.append(val1)
00662       start = end
00663       end += 4
00664       (length,) = _struct_I.unpack(str[start:end])
00665       pattern = '<%sd'%length
00666       start = end
00667       end += struct.calcsize(pattern)
00668       self.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00669       start = end
00670       end += 4
00671       (length,) = _struct_I.unpack(str[start:end])
00672       pattern = '<%sd'%length
00673       start = end
00674       end += struct.calcsize(pattern)
00675       self.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00676       start = end
00677       end += 4
00678       (length,) = _struct_I.unpack(str[start:end])
00679       pattern = '<%sd'%length
00680       start = end
00681       end += struct.calcsize(pattern)
00682       self.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00683       _x = self
00684       start = end
00685       end += 12
00686       (_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00687       start = end
00688       end += 4
00689       (length,) = _struct_I.unpack(str[start:end])
00690       start = end
00691       end += length
00692       if python3:
00693         self.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
00694       else:
00695         self.grasp.grasp_posture.header.frame_id = str[start:end]
00696       start = end
00697       end += 4
00698       (length,) = _struct_I.unpack(str[start:end])
00699       self.grasp.grasp_posture.name = []
00700       for i in range(0, length):
00701         start = end
00702         end += 4
00703         (length,) = _struct_I.unpack(str[start:end])
00704         start = end
00705         end += length
00706         if python3:
00707           val1 = str[start:end].decode('utf-8')
00708         else:
00709           val1 = str[start:end]
00710         self.grasp.grasp_posture.name.append(val1)
00711       start = end
00712       end += 4
00713       (length,) = _struct_I.unpack(str[start:end])
00714       pattern = '<%sd'%length
00715       start = end
00716       end += struct.calcsize(pattern)
00717       self.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00718       start = end
00719       end += 4
00720       (length,) = _struct_I.unpack(str[start:end])
00721       pattern = '<%sd'%length
00722       start = end
00723       end += struct.calcsize(pattern)
00724       self.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00725       start = end
00726       end += 4
00727       (length,) = _struct_I.unpack(str[start:end])
00728       pattern = '<%sd'%length
00729       start = end
00730       end += struct.calcsize(pattern)
00731       self.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00732       _x = self
00733       start = end
00734       end += 12
00735       (_x.grasp.grasp_pose.header.seq, _x.grasp.grasp_pose.header.stamp.secs, _x.grasp.grasp_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00736       start = end
00737       end += 4
00738       (length,) = _struct_I.unpack(str[start:end])
00739       start = end
00740       end += length
00741       if python3:
00742         self.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
00743       else:
00744         self.grasp.grasp_pose.header.frame_id = str[start:end]
00745       _x = self
00746       start = end
00747       end += 76
00748       (_x.grasp.grasp_pose.pose.position.x, _x.grasp.grasp_pose.pose.position.y, _x.grasp.grasp_pose.pose.position.z, _x.grasp.grasp_pose.pose.orientation.x, _x.grasp.grasp_pose.pose.orientation.y, _x.grasp.grasp_pose.pose.orientation.z, _x.grasp.grasp_pose.pose.orientation.w, _x.grasp.grasp_quality, _x.grasp.approach.direction.header.seq, _x.grasp.approach.direction.header.stamp.secs, _x.grasp.approach.direction.header.stamp.nsecs,) = _struct_8d3I.unpack(str[start:end])
00749       start = end
00750       end += 4
00751       (length,) = _struct_I.unpack(str[start:end])
00752       start = end
00753       end += length
00754       if python3:
00755         self.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
00756       else:
00757         self.grasp.approach.direction.header.frame_id = str[start:end]
00758       _x = self
00759       start = end
00760       end += 44
00761       (_x.grasp.approach.direction.vector.x, _x.grasp.approach.direction.vector.y, _x.grasp.approach.direction.vector.z, _x.grasp.approach.desired_distance, _x.grasp.approach.min_distance, _x.grasp.retreat.direction.header.seq, _x.grasp.retreat.direction.header.stamp.secs, _x.grasp.retreat.direction.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
00762       start = end
00763       end += 4
00764       (length,) = _struct_I.unpack(str[start:end])
00765       start = end
00766       end += length
00767       if python3:
00768         self.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
00769       else:
00770         self.grasp.retreat.direction.header.frame_id = str[start:end]
00771       _x = self
00772       start = end
00773       end += 36
00774       (_x.grasp.retreat.direction.vector.x, _x.grasp.retreat.direction.vector.y, _x.grasp.retreat.direction.vector.z, _x.grasp.retreat.desired_distance, _x.grasp.retreat.min_distance, _x.grasp.max_contact_force,) = _struct_3d3f.unpack(str[start:end])
00775       start = end
00776       end += 4
00777       (length,) = _struct_I.unpack(str[start:end])
00778       self.grasp.allowed_touch_objects = []
00779       for i in range(0, length):
00780         start = end
00781         end += 4
00782         (length,) = _struct_I.unpack(str[start:end])
00783         start = end
00784         end += length
00785         if python3:
00786           val1 = str[start:end].decode('utf-8')
00787         else:
00788           val1 = str[start:end]
00789         self.grasp.allowed_touch_objects.append(val1)
00790       _x = self
00791       start = end
00792       end += 8
00793       (_x.goal, _x.max_contact_force,) = _struct_if.unpack(str[start:end])
00794       return self
00795     except struct.error as e:
00796       raise genpy.DeserializationError(e) #most likely buffer underfill
00797 
00798 _struct_I = genpy.struct_I
00799 _struct_8d3I = struct.Struct("<8d3I")
00800 _struct_3I = struct.Struct("<3I")
00801 _struct_if = struct.Struct("<if")
00802 _struct_3d3f = struct.Struct("<3d3f")
00803 _struct_3d2f3I = struct.Struct("<3d2f3I")


object_manipulation_msgs
Author(s): Matei Ciocarlie
autogenerated on Mon Oct 6 2014 02:58:11