set joint position of robot

[vitiennam]

Hi,

I need to set robot's joint position to make data, but when I use env.step(action) it is too slow to get there, I need 40 iterations to get there

Thanks

[emilio-cartoni]

Dear @kimchicanh , you can set the positions (and velocities) of the robot joints instantaneously by using the reset_current_position(pos, vel) method.

Example:

import numpy as np
import real_robots
import gym

env = gym.make('REALRobot-v0')
env.render('human')
env.reset()

pos = np.pi / 4
vel = 0
robot = env.robot

for i in range(7):
    robot.jdict["lbr_iiwa_joint_%d" % (i+1)].reset_current_position(pos, vel)

robot.jdict["base_to_finger00_joint"].reset_current_position(0, 0)
robot.jdict["base_to_finger10_joint"].reset_current_position(0, 0)
robot.jdict["finger00_to_finger01_joint"].reset_current_position(0, 0)
robot.jdict["finger10_to_finger11_joint"].reset_current_position(0, 0)

This is useful to create some data for debugging/training/testing algorithms during development; however, when making submissions for the competition, it is assumed that the controller does not have access to these function, and it should gather data for its learning by interacting normally with the environment (i.e. not resetting itself instantaneously).

[vitiennam]

@emilio-cartoni thanks you so much

[emilio-cartoni]

Notice that using this function, joints can be set to values that are normally impossible to reach: i.e. it is possible to set the joints beyond their (-pi/2, +pi/2) limits or to put the robot arm inside the table.

[vitiennam]

@emilio-cartoni Thanks for your information, I have a problem with target porsition of camera, How to change it?, I set self.set_eye("eye", eye_pos=[0.01, 0, 1.4], target_pos=[100,0,0]) but nothing happen Thanks

[emilio-cartoni]

Dear @kimchicanh , you can change the eye position as in the following:

import numpy as np
import real_robots
import gym
import matplotlib.pyplot as plt

env = gym.make('REALRobot-v0')
env.render('human')
env.reset()

obs, _, _, _ = env.step(np.zeros(9))
plt.figure()
plt.imshow(obs['retina'])
plt.title("Standard eye")

# Changing eye
env.set_eye("eye", eye_pos=[0.01, 0, 1.4], target_pos=[100,0,0]) 
env.reset()  # This is necessary after using set_eye

obs, _, _, _ = env.step(np.zeros(9))
plt.figure()
plt.imshow(obs['retina'])
plt.title("Modified eye")

The target_pos parameter has no influence for the retina observation, since when the retina image is computed, the target is always the table.

See get_retina in REALRobotEnv

    def get_retina(self):
        '''
        :return: the current rgb_array for the eye
        '''
        return self.eyes["eye"].render(
                                       self.robot.object_bodies["table"]
                                       .get_position())

which calls render in EyeCamera

    def render(self, *args, **kargs):
        if self.pitch_roll is True:
            return self.renderPitchRoll(*args, **kargs)
        else:
            return self.renderTarget(*args, **kargs)

and then calls renderTarget in EyeCamera:

    def renderTarget(self, targetPosition, bullet_client=None):

        if bullet_client is None:
            bullet_client = self._p

        self.targetPosition = targetPosition

        view_matrix = bullet_client.computeViewMatrix(
                cameraEyePosition=self.eyePosition,
                cameraTargetPosition=self.targetPosition,
                cameraUpVector=self.upVector)

        proj_matrix = bullet_client.computeProjectionMatrixFOV(
                fov=self.fov,
                aspect=float(self.render_width)/self.render_height,
                nearVal=0.1, farVal=100.0)

        (_, _, px, _, _) = bullet_client.getCameraImage(
                width=self.render_width, height=self.render_height,
                viewMatrix=view_matrix,
                projectionMatrix=proj_matrix,
                renderer=pybullet.ER_BULLET_HARDWARE_OPENGL
                )

        rgb_array = np.array(px).reshape(self.render_height,
                                         self.render_width, 4)
        rgb_array = rgb_array[:, :, :3]

        return rgb_array

Beware that changing the eye position is not allowed for submissions :)