Ask Your Question
1

Wrong argument type when writing quaternion orientation to pose message

asked 2018-02-27 10:44:18 -0500

fitter gravatar image

updated 2018-03-05 07:23:30 -0500

Edit:

I tried to just simply copy the quaternion values and hardcode them as an orientation coordinates. Results are the same, so maybe problem comes from another part of my grasp msg? I dumped the message which I'm trying to use to pick my object: link text


Hi, I'm trying to generate moveit_msgs/Grasp Message using Python API and I'm getting ROSSerializationException in place where calculated quaternion values are assigned into geometry_msgs/Pose Message/orientation.

Piece of code where I'm making Grasp message and trying to get quaternion from euler angles:

p = PickPlaceInterface("arm", "gripper", verbose=True)
g = Grasp()

g.id = "test"
gp = PoseStamped()
gp.header.frame_id = "base_link"
gp.pose.position.x = grasps[0].surface.x
gp.pose.position.y = grasps[0].surface.y
gp.pose.position.z = grasps[0].surface.z

quat = quaternion_from_euler(grasps[0].approach.x, grasps[0].approach.y, grasps[0].approach.z)

print ("Quat type: " + str(type(quat[0])))
print ("Pose orient type: " + str(type(gp.pose.orientation.x)))

gp.pose.orientation.x = float(quat[0]) 
gp.pose.orientation.y = float(quat[1])
gp.pose.orientation.z = float(quat[2])
gp.pose.orientation.w = float(quat[3])
g.grasp_pose = gp

g.pre_grasp_approach.direction.header.frame_id = "base_link"
g.pre_grasp_approach.direction.vector.x = 1.0 
g.pre_grasp_approach.direction.vector.y = 0.0 
g.pre_grasp_approach.direction.vector.z = 0.0 
g.pre_grasp_approach.min_distance = 0.001
g.pre_grasp_approach.desired_distance = 0.1 

g.pre_grasp_posture.header.frame_id = "base_link"
g.pre_grasp_posture.joint_names = ["gripper_axis"]
pos = JointTrajectoryPoint()
pos.positions.append(0.0)
g.pre_grasp_posture.points.append(pos)

g.grasp_posture.header.frame_id = "base_link"
g.grasp_posture.joint_names = ["gripper_axis"]
pos = JointTrajectoryPoint()
pos.positions.append(0.02)
pos.effort.append(0.0)
g.grasp_posture.points.append(pos)

g.allowed_touch_objects = []
g.max_contact_force = 0
g.grasp_quality = grasps[0].score

p.pickup("obj", [g, ], support_name="supp")

grasps[0].approach is a geometry_msgs/Vector3 message.

grasps is a list of GraspConfig.msg which is defined here: link text

I'm getting this exception:

 Traceback (most recent call last):   File "pick_and_place.py", line 83, in <module>
    err = p.pickup("obj", [g, ], support_name="supp")   File "/opt/ros/kinetic/lib/python2.7/dist-packages/moveit_python/pick_place_interface.py", line 154, in pickup
    self._pick_action.send_goal(g)   File "/opt/ros/kinetic/lib/python2.7/dist-packages/actionlib/simple_action_client.py", line 92, in send_goal
    self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback)   File "/opt/ros/kinetic/lib/python2.7/dist-packages/actionlib/action_client.py", line 553, in send_goal
    return self.manager.init_goal(goal, transition_cb, feedback_cb)   File "/opt/ros/kinetic/lib/python2.7/dist-packages/actionlib/action_client.py", line 466, in init_goal
    self.send_goal_fn(action_goal)   File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 886, in publish
    raise ROSSerializationException(str(e)) rospy.exceptions.ROSSerializationException: <class 'struct.error'>: 'required argument is not a float' when writing 'x: 0.008194649141243238 y:
0.1413781390046223 z: 0.44856469150891914 w: 0.8824595101581431'

Looks simple, I'm just messing with types. But when I checked them with:

print ("Quat type: " + str(type(float(quat[0]))))
print ("Pose orient type: " + str(type ...
(more)
edit retag flag offensive close merge delete

Comments

What is type(quat[0])) ?

NEngelhard gravatar imageNEngelhard ( 2018-02-28 01:48:00 -0500 )edit

numpy.float64

fitter gravatar imagefitter ( 2018-02-28 02:39:03 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-03-05 07:20:18 -0500

fitter gravatar image

updated 2018-03-05 07:31:13 -0500

Ok, I figured out whats wrong. The problem is trivial, but because of the exception handling it was quite complicated to figure out whats wrong there.

Orientation values are completely OK, exception rises when grasp_quality value is serialized. I'm returning std_msgs/Float32 as a grasp_quality value. Fix is very easy:

g.grasp_quality = grasps[0].score #WRONG! returns std_msgs/Float32
g.grasp_quality = grasps[0].score.data #returns float

Now interesting part. Why exception returns orientation values? Lets look how that data is serialized in _PickupActionGoal.py (autogenerated code :/):

_v14 = _v12.orientation
_x = _v14 
buff.write(_get_struct_4d().pack(_x.x, _x.y, _x.z, _x.w))
buff.write(_get_struct_d().pack(val1.grasp_quality) #boom, error

#and part where exception is handled
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self)))))

Simply error printing uses _x to provide information where exception occured, but before writing grasp_quality value to buff _x is not updated, so it still contains orientation values, and those values are printed...

For me that looks like a bug, IMO _x should be updated before every buff.write(...).

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-02-27 10:44:18 -0500

Seen: 212 times

Last updated: Mar 05 '18