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
