コード例 #1
0
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
コード例 #2
0
 def position(self, q, index):
     self.q_def[NQ_OFFSET:] = q.reshape(self.nq - NQ_OFFSET)
     return RobotWrapper.placement(self, self.q_def, index)