Orientation constraint in moveit

asked 2022-02-22 13:46:13 -0500

madcat gravatar image

Hi, I was trying to do orientational constraint planning for a panda arm. I added enforce_joint_model_state_space into the panda arm config section in the ompl_planning.yaml and the end-effector flips in the path.

Here is how I add the constraints. Could anyone take a look if I did something wrong?

orien_const = OrientationConstraint()
orien_const.link_name = self.move_group.get_end_effector_link()
orien_const.header.frame_id = 'panda_link0'

orien_const.orientation.x = q[0] # q is the quarternion of target orientation
orien_const.orientation.y = q[1]
orien_const.orientation.z = q[2]
orien_const.orientation.w = q[3]

orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 2 * pi # ignore yaw axis
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
self.move_group.set_path_constraints(consts)

Thank you!

edit retag flag offensive close merge delete