We have to create a table and a grasp object similar to the robot environment. Here we are creating a table almost the same as in the gazebo simulation. If the table size and the pose of gazebo and MoveIt! are same, we can set the position of the grasping object:
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() #Table position p.pose.position.x = 0.45 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/grasp_table/model.sdf, using the values # for the surface link. self._scene.add_box(name, ...
