print "The robot is display with end effector on the red box."
sleep( 2 )
###
### MOVE #############################################################
###
print "Let's start the movement ..."
# Random velocity of the robot driving the movement
vq = zero( NV )
vq[ 0 ] = 2
vq[ 3 ] = 4
idx = robot.index( 'wrist_3_joint' )
oMeff = robot.placement( q, idx ) # Placement of end-eff wrt world at current configuration
oMbox = XYZQUATToSe3( q_box ) # Placement of box wrt world
effMbox = oMeff.inverse()*oMbox # Placement of box wrt eff
# Shortcut function to convert SE3 to 7-dof vector.
M2gv = lambda M: se3ToXYZQUAT(M)
for i in range(10000):
# Chose new configuration of the robot
q += vq / 40
q[ 2 ] = 1.71 + math.sin( i * 0.05 ) / 2
# Gets the new position of the box
oMbox = robot.placement( q, idx )*effMbox
# Display new configuration for robot and box
def position(self, q, index):
self.q_def[NQ_OFFSET:] = q.reshape(self.nq - NQ_OFFSET)
return RobotWrapper.placement(self, self.q_def, index)
