def Graspit(): GraspitCommander.clearWorld() Rotation = tf.transformations.quaternion_from_euler(0, math.pi / 2, 0) object_pose = geometry_msgs.msg.Pose() object_pose.position.x = 3 object_pose.position.y = 3 object_pose.position.z = 0.09 object_pose.orientation.x = Rotation[0] object_pose.orientation.y = Rotation[1] object_pose.orientation.z = Rotation[2] object_pose.orientation.w = Rotation[3] '''robot_transformation = np.array([[math.cos(67.5),0,math.sin(67.5),0],[0,1,0,0],[-math.sin(67.5),0,math.cos(67.5),0],[0,0,0,1]]) robot_rotation = tf.transformations.quaternion_from_matrix(robot_transformation) robot_pose = geometry_msgs.msg.Pose() robot_pose.position.x = -3 robot_pose.position.y = -3 robot_pose.position.z = 0.01 robot_pose.orientation.x = 0 robot_pose.orientation.y = 0 robot_pose.orientation.z = 0 robot_pose.orientation.w = 0''' GraspitCommander.importObstacle("floor") GraspitCommander.importGraspableBody("longBox", pose=object_pose) GraspitCommander.importRobot("fetch_gripper") plan = GraspitCommander.planGrasps(max_steps=50000) return plan
def graspit(): GraspitCommander.clearWorld() rotation = tf.transformations.quaternion_from_euler( 1.5707963, 0, -2.094129) poses = geometry_msgs.msg.Pose() poses.position.x = 3 poses.position.y = 3 poses.position.z = 0.001 poses.orientation.x = rotation[0] poses.orientation.y = rotation[1] poses.orientation.z = rotation[2] poses.orientation.w = rotation[3] GraspitCommander.importObstacle("floor") GraspitCommander.importGraspableBody("cracker", pose=poses) GraspitCommander.importRobot("fetch_gripper") plan = GraspitCommander.planGrasps(max_steps=50000) return plan
math.sin(67.5), 0], [0, 1, 0, 0], [-math.sin(67.5), 0, math.cos(67.5), 0], [0, 0, 0, 1]]) robot_rotation = tf.transformations.quaternion_from_matrix( robot_transformation) robot_pose = geometry_msgs.msg.Pose() robot_pose.position.x = -3 robot_pose.position.y = -3 robot_pose.position.z = 0.01 robot_pose.orientation.x = 0 robot_pose.orientation.y = 0 robot_pose.orientation.z = 0 robot_pose.orientation.w = 0 GraspitCommander.importObstacle("floor") GraspitCommander.importGraspableBody("longBox", pose=object_pose) GraspitCommander.importRobot("fetch_gripper", pose=robot_pose) GraspitCommander.planGrasps(max_steps=50000)
def setup_scene_with_barrett_hand(self, add_collision_plane=False): self.clear_world() if add_collision_plane: GraspitCommander.importObstacle("floor") self.import_robot("BarrettBH8_280")