def create_hinge_scene(): size = [0.5, 0.25, 1] b1, b2 = create_bodies([0, 0, 0], [0, 0, -2.5], size) demoutils.sim().add(b1) demoutils.sim().add(b2) f1 = agx.Frame() f1.setLocalTranslate(0, 0, size[2]) f1.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0)) hinge1 = agx.Hinge(b1, f1) demoutils.sim().add(hinge1) # Make the first motor swing back and forth speed_controller = AlternatingSpeedController(hinge1.getMotor1D(), 1, 2) demoutils.sim().add(speed_controller) distance = (b2.getPosition() - b1.getPosition()).length() f1 = agx.Frame() f1.setLocalTranslate(0, 0, -distance / 2) f1.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0)) f2 = agx.Frame() f2.setLocalTranslate(0, 0, distance / 2) f2.setLocalRotate(agx.EulerAngles(0, math.radians(90), 0)) hinge2 = agx.Hinge(b1, f1, b2, f2) demoutils.sim().add(hinge2)
def __init__(self, num_modules: int, pitch_only=False): super().__init__() self.material = agx.Material("snake_material_{}".format( self.getUuid().__str__)) self.modules = [] for i in range(0, num_modules): module = SnakeModule(self.material) module.setPosition(module_len * i, 0, 0) module.setRotation(agx.EulerAngles(math.pi / 2, 0, 0)) if not pitch_only: if i % 2 != 0: module.setRotation(agx.EulerAngles(0, 0, 0)) if i > 0: merged_body = agx.MergedBody() merged_body.add( agx.MergedBodyEmptyEdgeInteraction( self.modules[i - 1].upper, module.bottom)) snakeapp.add(merged_body) self.modules.append(module) self.add(module)
def build_tanks(self, material): length = self.rov_volume[0] / 5 rad = self.rov_volume[2] / 3 tank1 = self.cylinders(length, rad) tank1.setMaterial(material) tank2 = tank1.clone() tank1.setRotation(agx.EulerAngles(0, 0, 1 / 2 * math.pi)) tank2.setRotation(agx.EulerAngles(0, math.pi, math.pi * 3 / 2)) x, y, z = length / 1.5, rad * 1.5, rad * 3.5 tank1.setPosition(x, +y, z) tank2.setPosition(x, -y, z) return tank1, tank2
def build_geomery(shape: agxCollide.Shape, name: str, material, rot) -> agxCollide.Geometry: geom = agxCollide.Geometry(shape) geom.setMaterial(material) geom.setName(name) geom.setRotation(agx.EulerAngles(*rot)) return geom
def __init__(self, material): props = BrickProps() props.length *= 2 props.mass *= 3 super(Duplo4x2Cone, self).__init__(props) self.add_top(props, material) self.add_walls(props, material) self.add_center(props, material) self.add_center(props, material, x=props.stud_centerdist * 2) self.add_center(props, material, x=-props.stud_centerdist * 2) self.add_extrudes(props, material) self.add_stud(props, material, 3 * props.stud_centerdist, props.stud_centerdist) self.add_stud(props, material, 3 * props.stud_centerdist, -props.stud_centerdist) self.add_stud(props, material, -3 * props.stud_centerdist, props.stud_centerdist) self.add_stud(props, material, -3 * props.stud_centerdist, -props.stud_centerdist) #Add cone base = agxCollide.Geometry(agxCollide.Box((props.width - 0.001) / 2, (props.width - 0.001) / 2, props.studcyl_height * 1.5)) self.brick_rb.add(base) base.setLocalPosition(0, 0, props.height + props.studcyl_height * 1.5) base.setMaterial(material) cone_topradius = 2 * props.stud_radius cone_bottomradius = (props.width -0.001) / 2 cone_height = 0.038 cone = agxCollide.Geometry(agxCollide.Cone(cone_topradius, cone_bottomradius, cone_height)) cone.setMaterial(material) rotate_x(cone, 90) self.brick_rb.add(cone) cone.setLocalPosition(0, 0, props.height) self.add_stud(props, material, x=0, y=0, z=cone_height) self.height += cone_height #Add cone height self.brick_rb.setRotation(agx.EulerAngles(0, 0, math.radians(-90)))
def build_scene(): snakeapp.register_additional_scenes('build_scene2') angle = 0 for y in [-1, 0, 1]: angle += 10 obstacle = Obstacle(0.5, angle) obstacle.setLocalPosition(0, y, 0) snakeapp.add(obstacle) snake = Snake(5) snake.setLocalPosition(-0.5, y, 0.05) snake.setLocalRotation(agx.EulerAngles(math.pi / 2, 0, 0)) snakeapp.add(snake) snake_obstacle_cm = snakeapp.sim().getMaterialManager() \ .getOrCreateContactMaterial(snake.material, obstacle.material) # type: agx.ContactMaterial snake_obstacle_cm.setFrictionCoefficient(2) # a lower makes contacts more slippery snake_obstacle_cm.setUseContactAreaApproach(True) snake_obstacle_cm.setYoungsModulus(3E9) fm = agx.IterativeProjectedConeFriction(agx.FrictionModel.DIRECT) snake_obstacle_cm.setFrictionModel(fm) for i in range(0, snake.num_servos): sm = ExampleSineMotion(snake, i) if i % 2 == 0: sm.amplitude = math.radians(-90) else: sm.amplitude = math.radians(30) snakeapp.add_event_listener(sm)
def update_pos(self): """ Update local variables for position and rotation """ self.current_pos_x = self.gripper.getRigidBodies()[0].getPosition().x() self.current_pos_y = self.gripper.getRigidBodies()[1].getPosition().y() self.current_pos_z = self.gripper.getRigidBodies()[2].getPosition().z( ) - self.z_offs self.current_rot_z = agx.EulerAngles( self.gripper.getRigidBodies()[2].getRotation()).z()
def create_cylinder(position: agx.Vec3, scale): shape = agxCollide.Cylinder(0.5 * scale, 1 * scale) geometry = agxCollide.Geometry(shape) body = agx.RigidBody(geometry) body.setPosition(position) oneLegRobotApp.create_visual(body) body.setRotation(agx.EulerAngles(agx.PI_2, 0, 0)) return body
def build_rigid_body(geom: agxCollide.Geometry, name: str, pos, rot) -> agx.RigidBody: body = agx.RigidBody() body.add(geom) body.setName(name) body.setPosition(*pos) body.setRotation(agx.EulerAngles(*rot)) return body
def add_second_joint_aft(): geometry = agxCollide.Geometry(second_joint_shape_aft.deepCopy(), agx.AffineMatrix4x4.translate(0, 0, 100)) geometry.setLocalRotation(agx.EulerAngles(0, 0, math.pi / 2)) print(geometry.getRotation()) geometry.setEnableCollisions(True) oneLegRobotApp.create_visual(geometry, agxRender.Color.Red()) oneLegRobotApp.add(geometry)
def build_scene3(): create_and_add_ground() servo = create_and_add_servo() servo.setPosition(0, 0, 0.085) servo.setRotation(agx.EulerAngles(0, -math.pi / 2, 0)) controller = SimpleServoPositionController(servo) demoutils.sim().addEventListener(controller)
def create_cylinder(position: agx.Vec3): shape = agxCollide.Cylinder(0.5, 1) geometry = agxCollide.Geometry(shape) body = agx.RigidBody(geometry) body.setPosition(position) demoutils.create_visual(body) body.setRotation(agx.EulerAngles(agx.PI_2, 0, 0)) return body
def add_section_old(x, y, z, rotx, roty, rotz, path): geometry = agxCollide.Geometry(path.deepCopy(), agx.AffineMatrix4x4.translate(x, y, z)) geometry.setLocalRotation(agx.EulerAngles(rotx, roty, rotz)) #print(path + " were added with rotation " + geometry.getRotation()) geometry.setEnableCollisions(True) oneLegRobotApp.create_visual(geometry, agxRender.Color.Red()) #oneLegRobotApp.add(geometry) return geometry
def create_section(self, x, y, z, rotx, roty, rotz, path): geometry = agxCollide.Geometry(path.deepCopy(), agx.AffineMatrix4x4.translate(x, y, z)) geometry.setLocalRotation(agx.EulerAngles(rotx, roty, rotz)) geometry.setEnableCollisions(True) rigidBody = agx.RigidBody(geometry) rigidBody.setMotionControl(agx.RigidBody.DYNAMICS) oneLegRobotApp.create_visual(rigidBody, agxRender.Color.Red()) return rigidBody
def add_slope(): half_extents = agx.Vec3(length, 0.1, 0.005) slope = agxCollide.Geometry(agxCollide.Box(half_extents), agx.AffineMatrix4x4.translate(half_extents.x(), 0, -half_extents.z())) slope.setRotation(agx.EulerAngles(0, -self._slope_angle, 0)) snakeapp.create_visual(slope, diffuse_color=agxRender.Color.Red()) slope.setMaterial(self.material) self.add(slope) return slope
def create_slope() -> agx.RigidBody: slope_body = agx.RigidBody(agxCollide.Geometry(agxCollide.Box(1, 1, 0.05))) snakeapp.create_visual(slope_body, diffuse_color=Color.Red()) # slope_body.setPosition(agx.Vec3(0, 0, 0)) slope_body.setRotation(agx.EulerAngles(0, -math.radians(slope_angle), 0)) # Rotate 30 deg around Y. slope_body.setMotionControl(agx.RigidBody.STATIC) material = agx.Material("slopeMaterial") slope_body.getGeometries()[0].setMaterial(material) return slope_body
def add_first_joint_aft(): geometry = agxCollide.Geometry(first_joint_shape_aft.deepCopy(), agx.AffineMatrix4x4.translate(0, 0, 200)) geometry.setLocalRotation(agx.EulerAngles(0, 0, math.pi / 2)) print(geometry.getRotation()) geometry.setEnableCollisions(True) rigidBody = agx.RigidBody(geometry) oneLegRobotApp.create_visual(rigidBody, agxRender.Color.Red()) oneLegRobotApp.add(rigidBody) rigidBody.setMotionControl(agx.RigidBody.STATIC)
def build_scene(): snakeapp.register_additional_scenes('build_scene2') slope = create_slope() snakeapp.add(slope) snake = Snake(5) snake.setPosition(0, 0, 0.1) snake.setRotation(agx.EulerAngles(0, -math.radians(slope_angle), 0.2)) snakeapp.add(snake)
def create_floor(): w = 200 b = 200 h = 10 #floor = agxCollide.Geometry(agxCollide.Box(2.5, 0.5, h), agx.AffineMatrix4x4.translate(0, 0, -h)) floor = agxCollide.Geometry(agxCollide.Box(w, b, h)) floor.setPosition(0, 0, 0) floor.setRotation(agx.EulerAngles(0, 0, 0)) oneLegRobotApp.create_visual(floor, diffuse_color=agxRender.Color.Green()) oneLegRobotApp.add(floor)
def build_scene2(): slope = create_slope() snakeapp.add(slope) snake = Snake(5) snake.setPosition(0, 0, 0.1) snake.setRotation(agx.EulerAngles(0, -math.radians(slope_angle), 0.2)) snakeapp.add(snake) snake_obstacle_cm = snakeapp.sim().getMaterialManager() \ .getOrCreateContactMaterial(snake.material, slope.getGeometries()[0].getMaterial()) # type: agx.ContactMaterial snake_obstacle_cm.setFrictionCoefficient(10) snake_obstacle_cm.setYoungsModulus(3E4)
def create_floor(self): w = 200 b = 200 h = 10 # floor = agxCollide.Geometry(agxCollide.Box(2.5, 0.5, h), agx.AffineMatrix4x4.translate(0, 0, -h)) floor = agxCollide.Geometry(agxCollide.Box(w, b, h)) floor.setPosition(0, 0, 0) floor.setRotation(agx.EulerAngles(0, 0, 0)) floor.setEnableCollisions(True) rigidBody = agx.RigidBody(floor) rigidBody.setMotionControl(agx.RigidBody.STATIC) oneLegRobotApp.create_visual(rigidBody, diffuse_color=agxRender.Color.Green()) return floor
def __init__(self, material, listener_pool): props = BrickProps() props.height /= 2 #Height of lower part props.length *= 4 props.mass *= 4 props.update() super(Duplo8x2plus2x2, self).__init__(props) #Low 8x2 brick self.add_top(props, material) self.add_walls(props, material) self.add_center(props, material) self.add_center(props, material, x=props.stud_centerdist * 2) self.add_center(props, material, x=-props.stud_centerdist * 2) self.add_center(props, material, x=props.stud_centerdist * 4) self.add_center(props, material, x=-props.stud_centerdist * 4) self.add_center(props, material, x=props.stud_centerdist * 6) self.add_center(props, material, x=-props.stud_centerdist * 6) self.add_extrudes(props, material) self.add_studs(props, material, props.stud_centerdist * 2, 0, listener_pool) self.add_studs(props, material, -props.stud_centerdist * 2, 0, listener_pool) self.add_studs(props, material, -props.stud_centerdist * 6, 0, listener_pool) self.add_studs(props, material, props.stud_centerdist * 6, 0, listener_pool) #Needed to get mass disbribution correct #2x2 bricks props.height *= 2 props.length /= 4 props.update() x_offset = -props.length * 2 z_offset = props.height / 4 self.add_top(props, material, x=x_offset, y=0, z=z_offset) self.add_walls(props, material, x=x_offset, y=0, z=z_offset) self.add_center(props, material, x=x_offset, y=0, z=z_offset) #Needed to get mass disbribution correct self.add_stud(props, material, x_offset + props.stud_centerdist, props.stud_centerdist, z=z_offset) self.add_stud(props, material, x_offset + props.stud_centerdist, -props.stud_centerdist, z=z_offset) x_offset = -props.length * 2.5 z_offset += props.height self.add_top(props, material, x=x_offset, y=0, z=z_offset) self.add_walls(props, material, x=x_offset, y=0, z=z_offset) self.add_center(props, material, x=x_offset, y=0, z=z_offset) #Needed to get mass disbribution correct self.add_stud(props, material, x_offset + props.stud_centerdist, props.stud_centerdist, z=z_offset) self.add_stud(props, material, x_offset + props.stud_centerdist, -props.stud_centerdist, z=z_offset) self.add_stud(props, material, x_offset - props.stud_centerdist, props.stud_centerdist, z=z_offset) self.add_stud(props, material, x_offset - props.stud_centerdist, -props.stud_centerdist, z=z_offset) self.brick_rb.setRotation(agx.EulerAngles(0, 0, math.radians(-90)))
def __init__(self, material, listener_pool): props = BrickProps() props.length *= 2 props.mass *= 2 super(Duplo4x2, self).__init__(props) self.add_top(props, material) self.add_walls(props, material) self.add_center(props, material) self.add_center(props, material, x=props.stud_centerdist * 2) self.add_center(props, material, x=-props.stud_centerdist * 2) self.add_extrudes(props, material) self.add_studs(props, material, props.stud_centerdist * 2, 0, listener_pool) self.add_studs(props, material, -props.stud_centerdist * 2, 0, listener_pool) self.brick_rb.setRotation(agx.EulerAngles(0, 0, math.radians(-90)))
def __init__(self, material: agx.Material = None): super().__init__() self.len = (module_len * 0.5) + intermediate_len self.upper = UpperPart() self.intermediate = IntermediatePart(material) self.intermediate.setRotation(agx.EulerAngles(math.pi, 0, math.pi)) self.intermediate.setPosition(agx.Vec3(module_len / 2 + intermediate_len, 0, 0)) self.add(self.upper) self.add(self.intermediate) merged_body = self.intermediate.merged_body merged_body.add(agx.MergedBodyEmptyEdgeInteraction(self.upper, self.intermediate.body)) snakeapp.add(merged_body)
def __init__(self): super().__init__() len1 = 2 len2 = 1 self.link1 = create_link(len1) self.link2 = create_link(len2) self.link2.setPosition(len2, 0, len1) self.link2.setRotation(agx.EulerAngles(0, math.radians(-90), 0)) self.link1.getGeometries()[0].setEnableCollisions(self.link2.getGeometries()[0], False) demoutils.create_visual(self.link1) demoutils.create_visual(self.link2) self.hinge = demoutils.create_constraint( pos=agx.Vec3(width, 0, len1), axis=agx.Vec3(0, 1, 0), rb1=self.link1, rb2=self.link2, c=agx.Hinge) # type: agx.Hinge self.hinge.setCompliance(1e-8) self.hinge.getLock1D().setEnable(False) self.hinge.getMotor1D().setEnable(False) self.hinge.getRange1D().setEnable(True) self.hinge.getRange1D().setRange(math.radians(-45), math.radians(75)) f1 = agx.Frame() f1.setTranslate(agx.Vec3(width, 0, len1 - (len1 * 0.7))) f2 = agx.Frame() f2.setTranslate(agx.Vec3(-width, 0, 0)) self.distance = agx.DistanceJoint(self.link1, f1, self.link2, f2) self.distance.getMotor1D().setEnable(False) self.distance.getLock1D().setEnable(True) self.add(self.link1) self.add(self.link2) self.add(self.hinge) self.add(self.distance)
def build_scene(): snakeapp.create_sky() snake = Snake(6) snake.setRotation(agx.EulerAngles(math.pi / 2, 0, 0)) snake.setPosition(agx.Vec3(0, .5, 0.3)) snakeapp.add(snake) terrain = Terrain() terrain_snake_cm = snakeapp.sim().getMaterialManager() \ .getOrCreateContactMaterial(terrain.material, snake.material) # type: agx.ContactMaterial terrain_snake_cm.setFrictionCoefficient(3) terrain_snake_cm.setUseContactAreaApproach(True) terrain_snake_cm.setYoungsModulus(3E3) fm = agx.IterativeProjectedConeFriction(agx.FrictionModel.DIRECT) terrain_snake_cm.setFrictionModel(fm) for i in range(0, snake.num_servos): snakeapp.add_event_listener(ExampleSineMotion(snake, i)) snakeapp.init_camera(eye=agx.Vec3(-2, -2, 1), center=agx.Vec3(0, 0, 0.5)) import agxSDK class ReadSensor(agxSDK.StepEventListener): def __init__(self): super().__init__(agxSDK.StepEventListener.POST_STEP) def post(self, time): pass # for i in range(0, snake.num_sensors): # print("Sensor{} force={}".format(i, snakeapp.get_sum_force_magnitude(snake.sensors[i]))) # print("") snakeapp.add_event_listener(ReadSensor())
def rotate_x(geometry, degrees): """Rotate geometry around x axis """ geometry.setLocalRotation(agx.EulerAngles(math.radians(degrees), 0, 0))
def rotate_z(geometry, degrees): """Rotate geometry around z axis """ geometry.setLocalRotation(agx.EulerAngles(0, 0, math.radians(degrees)))
def setup_geometries(self, root, material, visual): # pylint: disable=too-many-statements, too-many-locals """ Create bodies and constraints so that we can move the gripper left/right/front/back """ translate_bodies = [agx.RigidBody(), agx.RigidBody()] move_direction = [agx.Vec3(-1, 0, 0), agx.Vec3(0, -1, 0)] for i in range(0, 2): body = translate_bodies[i] body.setLocalPosition(0, 0, 0) body.getMassProperties().setMass(self.props.mass) self.gripper.add(body) if i == 0: frame1 = agx.Frame() frame1.setLocalTranslate(body.getPosition()) frame1.setLocalRotate( agx.Quat(agx.Vec3(0, 0, -1), move_direction[i])) prismatic = agx.Prismatic(body, frame1) else: frame1 = agx.Frame() frame2 = agx.Frame() assert agx.Constraint.calculateFramesFromBody(agx.Vec3(), \ move_direction[i], translate_bodies[0], frame1, body, frame2) prismatic = agx.Prismatic(translate_bodies[0], frame1, body, frame2) self.gripper.add(prismatic) prismatic.getMotor1D().setLockedAtZeroSpeed(True) prismatic.getMotor1D().setForceRange(agx.RangeReal(50)) # N prismatic.getMotor1D().setSpeed(0) prismatic.getMotor1D().setEnable(True) prismatic.setSolveType(agx.Constraint.DIRECT_AND_ITERATIVE) prismatic.setCompliance(1e-15) self.move_constraints.append(prismatic) # Create a Cylindrical constraint that can be used for lifting AND rotating lift_rotate_body = agx.RigidBody() lift_rotate_body.setLocalPosition(0, 0, 0) lift_rotate_body.getMassProperties().setMass(self.props.mass) self.gripper.add(lift_rotate_body) frame1 = agx.Frame() frame2 = agx.Frame() assert agx.Constraint.calculateFramesFromBody( \ agx.Vec3(), agx.Vec3(0, 0, -1), translate_bodies[1], frame1, lift_rotate_body, frame2) self.cylindrical = agx.CylindricalJoint(translate_bodies[1], frame1, lift_rotate_body, frame2) self.cylindrical.setSolveType(agx.Constraint.DIRECT_AND_ITERATIVE) self.cylindrical.setCompliance(1e-15) # Enable the motors and set some properties on the motors for d in [agx.Constraint2DOF.FIRST, agx.Constraint2DOF.SECOND]: self.cylindrical.getMotor1D(d).setEnable(True) self.cylindrical.getMotor1D(d).setLockedAtZeroSpeed(True) self.cylindrical.getMotor1D(d).setSpeed(0) if d == agx.Constraint2DOF.FIRST: self.cylindrical.getMotor1D(d).setForceRange( agx.RangeReal(15)) # N else: self.cylindrical.getMotor1D(d).setForceRange( agx.RangeReal(50)) # Nm self.gripper.add(self.cylindrical) # Create the actual fingers that is used for picking up screws. capsule1 = agxCollide.Geometry( agxCollide.Capsule(self.props.radius, self.props.height)) capsule1.setLocalRotation(agx.EulerAngles(math.radians(90), 0, 0)) capsule1.setMaterial(material) capsule2 = capsule1.clone() capsule1.setLocalPosition(0, -self.props.radius, 0) capsule2.setLocalPosition(0, self.props.radius, 0) self.finger1 = agx.RigidBody() self.finger1.add(capsule1) self.finger1.add(capsule2) self.finger2 = self.finger1.clone() fingers = [self.finger1, self.finger2] # Create the constraints that is used for open/close the picking device direction = [1, -1] self.grip_constraints = [] grip_range = 0.025 self.grip_offs = self.props.radius for i in range(0, 2): finger = fingers[i] finger.setLocalPosition( direction[i] * (self.grip_offs + self.posref.grip1), 0, 0) self.gripper.add(finger) finger.getMassProperties().setMass(self.props.mass) frame1 = agx.Frame() frame2 = agx.Frame() assert agx.Constraint.calculateFramesFromBody( \ agx.Vec3(), agx.Vec3(-1, 0, 0) * direction[i], lift_rotate_body, frame1, finger, frame2) prismatic = agx.Prismatic(lift_rotate_body, frame1, finger, frame2) prismatic.getMotor1D().setForceRange(agx.RangeReal(-20, 20)) prismatic.getMotor1D().setLockedAtZeroSpeed(True) prismatic.getMotor1D().setEnable(True) prismatic.getRange1D().setRange( agx.RangeReal(-self.posref.grip1, grip_range - self.posref.grip1)) prismatic.getRange1D().setEnable(True) prismatic.setSolveType(agx.Constraint.DIRECT_AND_ITERATIVE) prismatic.setCompliance(1e-15) self.gripper.add(prismatic) self.grip_constraints.append(prismatic) self.sim.add(self.gripper) if visual: agxOSG.createVisual(self.gripper, root)
def _get_observation(self): rgb_buffer = None depth_buffer = None for buffer in self.render_to_image: name = buffer.getName() if name == 'rgb_buffer': rgb_buffer = buffer elif name == 'depth_buffer': depth_buffer = buffer assert self.observation_type in ("rgb", "depth", "rgb_and_depth", "pos", "pos_and_vel") if self.observation_type == "rgb": image_ptr = rgb_buffer.getImageData() image_data = create_numpy_array( image_ptr, (self.image_size[0], self.image_size[1], 3), np.uint8) obs = np.flipud(image_data) elif self.observation_type == "depth": image_ptr = depth_buffer.getImageData() image_data = create_numpy_array( image_ptr, (self.image_size[0], self.image_size[1]), np.float32) obs = np.flipud(image_data) elif self.observation_type == "rgb_and_depth": obs = np.zeros((self.image_size[0], self.image_size[1], 4), dtype=np.float32) image_ptr = rgb_buffer.getImageData() image_data = create_numpy_array( image_ptr, (self.image_size[0], self.image_size[1], 3), np.uint8) obs[:, :, 0:3] = np.flipud(image_data.astype(np.float32)) / 255 image_ptr = depth_buffer.getImageData() image_data = create_numpy_array( image_ptr, (self.image_size[0], self.image_size[1]), np.float32) obs[:, :, 3] = np.flipud(image_data) elif self.observation_type == "pos": seg_pos = get_cable_segment_positions( cable=agxCable.Cable.find(self.sim, "DLO")).flatten() gripper = self.sim.getRigidBody("gripper_body") gripper_pos = to_numpy_array(gripper.getPosition())[0:3] cylinder = self.sim.getRigidBody("hollow_cylinder") cylinder_pos = cylinder.getPosition() ea = agx.EulerAngles().set(gripper.getRotation()) gripper_rot = ea.y() obs = np.concatenate([ gripper_pos, [gripper_rot], seg_pos, [cylinder_pos[0], cylinder_pos[1]] ]) elif self.observation_type == "pos_and_vel": seg_pos, seg_vel = get_cable_segment_positions_and_velocities( cable=agxCable.Cable.find(self.sim, "DLO")) seg_pos = seg_pos.flatten() seg_vel = seg_vel.flatten() gripper = self.sim.getRigidBody("gripper_body") gripper_pos = to_numpy_array(gripper.getPosition())[0:3] gripper_vel = to_numpy_array(gripper.getVelocity())[0:3] gripper_vel_rot = to_numpy_array(gripper.getAngularVelocity())[2] cylinder = self.sim.getRigidBody("hollow_cylinder") cylinder_pos = cylinder.getPosition() ea = agx.EulerAngles().set(gripper.getRotation()) gripper_rot = ea.y() obs = np.concatenate([ gripper_pos, [gripper_rot], gripper_vel, [gripper_vel_rot], seg_pos, seg_vel, [cylinder_pos[0], cylinder_pos[1]] ]) return obs