def test_matrix_rpy(self): for mat, rpy in [ (mod.matrixToTuple(np.identity(4)), (0, 0, 0, 0, 0, 0)), (mod.matrixToTuple(-np.identity(4)), (0, 0, 0, -np.pi, 0, -np.pi)), ]: np.testing.assert_allclose(rpy, mod.matrixToRPY(mat)) # np.testing.assert_allclose(mat, mod.RPYToMatrix(rpy)) np.testing.assert_allclose(rpy, mod.matrixToRPY(mod.RPYToMatrix(rpy))) def test_rotate(self): for axis, angle, mat in [ ('x', np.pi, ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))), ('y', np.pi, ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))), ('z', np.pi, ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))), ]: self.assertEqual(mat, mod.rotate(axis, angle)) def test_quat_mat(self): for quat, mat in [ ((0, 0, 0, 1), np.identity(3)), ((0, 0, 1, 0), ((-1, 0, 0), (0, -1, 0), (0, 0, 1))), ((0, -0.5, 0, 0.5), ((0.5, 0, -0.5), (0, 1, 0), (0.5, 0, 0.5))), ]: self.assertEqual(mat, mod.quaternionToMatrix(quat))
def updateFeetCollision(): ''' Update the box for the feet ''' wMcomr = matrixToTuple(matrix(dyn.rf.value)*arMcomr) # world_M_ankleRight * anklRight_M_comr wMcoml = matrixToTuple(matrix(dyn.lf.value)*alMcoml) # wMar*arMcomr wMcomr_XYZ_RPY = matrixToRPY( wMcomr ) # com of RF box in (x,y,z,r,p,y) format wMcoml_XYZ_RPY = matrixToRPY( wMcoml ) # com of LF box in (x,y,z,r,p,y) format robot.viewer.updateElementConfig('boxRF', wMcomr_XYZ_RPY) # View the right foot box robot.viewer.updateElementConfig('boxLF', wMcoml_XYZ_RPY) # View the right foot box fclRf.setBoxTransf(wMcomr) # Set pose of RF box (for collision detection) fclLf.setBoxTransf(wMcoml) # Set pose of LF box (for collision detection) ''' Show contact points ''' # Prf = fclRf.contactPoints.value # Contact points 'Pc' in the world frame 'w' # Plf = fclLf.contactPoints.value # Contact points 'Pc' in the world frame 'w' Prf = gridRf.contactPoints.value # Contact points 'Pc' in the world frame 'w' Plf = gridLf.contactPoints.value # Contact points 'Pc' in the world frame 'w' for i in range(len(Prf)): # Show the contact points for the right foot name = 'point'+str(i+1) robot.viewer.updateElementConfig(name, [Prf[i][0],Prf[i][1],Prf[i][2],0,0,0]) for i in range(len(Plf)): # Show the contact points for the left foot name = 'point'+str(i+1+len(Prf)) robot.viewer.updateElementConfig(name, [Plf[i][0],Plf[i][1],Plf[i][2],0,0,0]) for i in range(NPt-len(Plf)-len(Prf)): # Hide the points that are not in contact name = 'point'+str(i+1+len(Prf)+len(Plf)) robot.viewer.updateElementConfig(name, [0,0,-1,0,0,0])
# ------------------------------------------------------------------------------ dyn.rf.recompute(0) dyn.lf.recompute(0) # --- Definitions for the feet sole --- soleRF = ((0.11,-0.08,-0.08,0.11),(-0.065,-0.065,0.045,0.045),(-0.105,-0.105,-0.105,-0.105)) soleLF = ((0.11,-0.08,-0.08,0.11),(0.065,0.065,-0.045,-0.045),(-0.105,-0.105,-0.105,-0.105)) arPcomr = matrix(( average(soleRF[0]), average(soleRF[1]), -0.105 + 0.012 )).T alPcoml = matrix(( average(soleLF[0]), average(soleLF[1]), -0.105 + 0.012 )).T arMcomr = bmat([ [mat(eye(3)),arPcomr], [mat(zeros(3)),mat(1.)] ]) alMcoml = bmat([ [mat(eye(3)),alPcoml], [mat(zeros(3)),mat(1.)] ]) wMcomr = matrixToTuple(matrix(dyn.rf.value)*arMcomr) # wMar*arMcomr wMcoml = matrixToTuple(matrix(dyn.lf.value)*alMcoml) # wMar*arMcomr wMcomr_XYZ_RPY = matrixToRPY( wMcomr ) # com of right foot box in (x,y,z,r,p,y) format wMcoml_XYZ_RPY = matrixToRPY( wMcoml ) # com of left foot box in (x,y,z,r,p,y) format # --- Collision detection with the floor ---- fclRf = FclBoxMeshCollision('fclRf') fclRf.setMinDistance(0.002) fclRf.setBoxDimensions((0.23, 0.135, 0.024)) fclRf.setBoxTransf(wMcomr) fclRf.setMeshFile('plane2mCentered_005.obj') fclRf.setMeshPose((0,0,0,0,0,0)) fclLf = FclBoxMeshCollision('fclLf') fclLf.setMinDistance(0.002) fclLf.setBoxDimensions((0.23, 0.135, 0.024)) fclLf.setBoxTransf(wMcoml)
def supervision(): global state, tool, robot, solver, pos_err_des, gainMax, gainMin, goal, p72tohole, m2sig if state == 0: robot.mTasks['lh'].feature.error.recompute(robot.device.control.time) if linalg.norm(array( robot.mTasks['lh'].feature.error.value)[0:3]) < pos_err_des: openGrippers(robot, solver) #state = 1000 #needed if you want to make the robot wait state = 1 print "time = " + str(robot.device.control.time) if state == 1: robot.device.state.recompute(robot.device.control.time) if linalg.norm( array([ robot.device.state.value[28] - robot.mTasks['posture'].ref[28], robot.device.state.value[35] - robot.mTasks['posture'].ref[35] ])) < 0.003: robot.device.viewer.updateElementConfig('TwoHandTool', tool) closeGrippers(robot, solver) state += 1 print "time = " + str(robot.device.control.time) if state == 2: robot.device.state.recompute(robot.device.control.time) if linalg.norm( array([ robot.device.state.value[28] - robot.mTasks['posture'].ref[28], robot.device.state.value[35] - robot.mTasks['posture'].ref[35] ])) < 0.003: print "Goal: " + str(array(goal.value)) #here there is the plug screw_2ht(robot, solver, tool, P72, goal, gainMax, gainMin) #write_pos_py("/opt/grx3.0/HRP2LAAS/script/airbus_robot/",robot.device.state.value[6:36]) state += 1 print "time = " + str(robot.device.control.time) if state >= 3 and state <= 19: robot.mTasks['screw'].feature.error.recompute( robot.device.control.time) if linalg.norm(array( robot.mTasks['screw'].feature.error.value)) < pos_err_des: print "state = " + str(state) if state < len(p72tohole) + 2: p72togoal.sin2.value = p72tohole[state - 2] print "Goal: " + str(array(goal)) #screw_2ht(robot,solver,tool,goal,gainMax, gainMin) goal.recompute(robot.device.control.time) robot.device.viewer.updateElementConfig( 'goal1', vectorToTuple(array(matrixToRPY(goal.value)))) if state == len(p72tohole) + 2: state = 20 else: state += 1 print "time = " + str(robot.device.control.time) if state == 20: robot.mTasks['screw'].feature.error.recompute( robot.device.control.time) if linalg.norm(array( robot.mTasks['screw'].feature.error.value)[0:3]) < pos_err_des: get_2ht(robot, solver, tool, gainMax, gainMin) state += 1 print "time = " + str(robot.device.control.time) if state == 21: robot.mTasks['lh'].feature.error.recompute(robot.device.control.time) if linalg.norm(array( robot.mTasks['lh'].feature.error.value)[0:3]) < pos_err_des: openGrippers(robot, solver) #state = 8000 #needed if you want to make the robot wait state = 22 print "time = " + str(robot.device.control.time) if state == 22: robot.device.state.recompute(robot.device.control.time) if linalg.norm( array([ robot.device.state.value[28] - robot.mTasks['posture'].ref[28], robot.device.state.value[35] - robot.mTasks['posture'].ref[35] ])) < 0.002: robot.device.viewer.updateElementConfig('TwoHandTool', (0., 0.5, 0., 0., 0., 0.)) goToHalfSitting(robot, solver, 5) state = 23 print "time = " + str(robot.device.control.time) # wait 200*dt (1 second) after state 1 if state >= 1000 and state < 1200: robot.device.state.recompute(robot.device.control.time) if linalg.norm( array([ robot.device.state.value[28] - robot.mTasks['posture'].ref[28], robot.device.state.value[35] - robot.mTasks['posture'].ref[35] ])) < 0.002: state += 1 if state == 1200: state = 1 print "time = " + str(robot.device.control.time) # wait 200*dt (1 second) after state 8 if state >= 21000 and state < 21200: robot.device.state.recompute(robot.device.control.time) if linalg.norm( array([ robot.device.state.value[28] - robot.mTasks['posture'].ref[28], robot.device.state.value[35] - robot.mTasks['posture'].ref[35] ])) < 0.002: state += 1 if state == 21200: state = 22 print "time = " + str(robot.device.control.time)
(0., 0.0, 0., 0., 0., 0.)) P72RPY = (0.75, -0.45, 0.85, 0., 0., 1.57) robot.device.viewer.updateElementConfig('P72', P72RPY) P72 = RPYToMatrix(P72RPY) #----------------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------- #----------------------------------------------------------------------------- p72togoal = Multiply_of_matrixHomo("p72togoal") p72togoal.sin1.value = matrixToTuple(P72) p72togoal.sin2.value = matrixToTuple(p72tohole[0]) goal = p72togoal.sout goal.recompute(0) robot.device.viewer.updateElementConfig( 'goal1', vectorToTuple(array(matrixToRPY(goal.value)))) get_2ht(robot, solver, tool, gainMax, gainMin) state = 0 """ # Motion recording zmp_out = open("/tmp/data.zmp","w") hip_out = open("/tmp/data.hip","w") pos_out = open("/tmp/data.pos","w") """
taskCom.gain.setConstant(0.1) taskPosture.ref = pose # Set the target for RH and LH task. Selec is the activation flag (say control only # the XYZ translation), and gain is the adaptive gain (10 at the target, 0.1 # far from it, with slope st. at 0.01m from the target, 90% of the max gain # value is reached displacementMatrix = eye(4) displacementMatrix[0:3, 3] = array([0.1, 0., 0.]) taskRH.feature.position.recompute(0) taskLH.feature.position.recompute(0) targetRH = vectorToTuple( array( matrixToRPY( dot(displacementMatrix, array(taskRH.feature.position.value))))) gotoNd(taskRH, targetRH, "111111", (50, 1, 0.01, 0.9)) RHPos = taskRH.feature.position.value LHPos = taskLH.feature.position.value #3RHPos = vectorToTuple(array(taskRH.feature.position.value)[0:3,3]) #LHPos = vectorToTuple(array(taskLH.feature.position.value)[0:3,3]) gotoNdRel(taskRel, RHPos, LHPos, '110111', (50, 1, 0.01, 0.9)) taskRel.feature.errordot.value = (0, 0, 0, 0, 0) # not to forget!! ############################################################ # Production of variable reference vector HToR = HomoToRotation("HToR") plug(taskLH.feature.position, HToR.sin)
p72tohole = getData(0.) tool = (0.4, -0.1, 0.8, 0., 0., pi / 2) P72RPY = (0.75, -0.45, 0.85, 0., 0., 1.57) P72 = RPYToMatrix(P72RPY) robot.device.viewer.updateElementConfig('TwoHandTool', (0., 0., 0., 0., 0., 0.)) robot.device.viewer.updateElementConfig('P72', P72RPY) robot.device.viewer.updateElementConfig( 'goal1', vectorToTuple(array(matrixToRPY(dot(P72, p72tohole[0]))))) #----------------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------- #----------------------------------------------------------------------------- get_2ht(robot, solver, tool, gainMax, gainMin) state = 0 """ # Motion recording zmp_out = open("/tmp/data.zmp","w") hip_out = open("/tmp/data.hip","w") pos_out = open("/tmp/data.pos","w") """
def displayFov(recompute=False): if recompute: taskGaze.proj.transfo.recompute(robot.state.time) robot.viewer.updateElementConfig('fov',matrixToRPY(taskGaze.proj.transfo.value))