def screw_2ht(robot, solver, tool, target, goal, gainMax, gainMin): #goal = array([0.5,-0.3,1.1,0.,1.57,0.]) if 'rel' not in robot.mTasks: createRelativeTask(robot) if 'screw' not in robot.mTasks: createScrewTask(robot, tool) createVecMult(robot) if not hasattr(robot, 'm2pos'): createM2Pos(robot) # Task Relative gotoNdRel(robot.mTasks['rel'], array(robot.mTasks['rh'].feature.position.value), array(robot.mTasks['lh'].feature.position.value), '110111', gainMax * 2) robot.mTasks['rel'].feature.errordot.value = (0, 0, 0, 0, 0 ) # not to forget!! # Aim setting if isinstance(goal, ndarray): if len(goal) == 6: refToGoalMatrix = RPYToMatrix(goal) else: refToGoalMatrix = goal robot.mTasks['screw'].ref = matrixToTuple(refToGoalMatrix) robot.mTasks['screw'].featureVec.positionRef.value = dot( refToGoalMatrix[0:3, 0:3], array([0., 0., 1.])) else: #goal is a signal plug(goal, robot.mTasks['screw'].featureDes.position) plug(goal, robot.selec.sin) robot.mult.sin2.value = (0., 0., 1.) plug(robot.mult.sout, robot.mTasks['screw'].featureVec.positionRef) if isinstance(target, ndarray): if len(target) == 6: refToTargetMatrix = RPYToMatrix(target) else: refToTargetMatrix = target robot.mTasks['gaze'].proj.point3D.value = vectorToTuple(target[0:3, 3]) else: #target is a signal plug(target, robot.m2pos.sin) plug(robot.m2pos.sout, robot.mTasks['gaze'].proj.point3D) robot.mTasks['gaze'].gain.setByPoint(gainMax * 2, gainMin * 2, 0.01, 0.9) robot.mTasks['screw'].gain.setByPoint(gainMax, gainMin, 0.01, 0.9) tasks = array([ robot.mTasks['rel'].task, robot.mTasks['gaze'].task, robot.mTasks['screw'].task ]) # sot charging if not (('taskrel' in solver.toList()) and ('taskgaze' in solver.toList()) and ('taskscrew' in solver.toList())): removeUndesiredTasks(solver) for i in range(len(tasks)): solver.push(tasks[i])
def createScrewTask(robot, TwoHandTool): refToTwoHandToolMatrix = array(RPYToMatrix(TwoHandTool)) #RH-TwoHandTool Homogeneous Transformation Matrix (fixed in time) robot.mTasks['rh'].feature.position.recompute(robot.device.control.time) RHToTwoHandToolMatrix = dot( linalg.inv(array(robot.mTasks['rh'].feature.position.value)), refToTwoHandToolMatrix) #!!!!!! RH and Support are different references, because the X rotation is not controlled in positioning!!!!!!!!!!!!!!!!!!!!!!!!!! RHToScrewMatrix = dot(RHToTwoHandToolMatrix, TwoHandToolToScrewMatrix) # Screw Lenght - unused at the moment #screw_len = 0.03 # TASK Screw robot.mTasks['screw'] = MetaTaskKine6d('screw', robot.dynamic, 'screw', 'right-wrist') handMgrip = array(robot.mTasks['rh'].opmodif) robot.mTasks['screw'].opmodif = matrixToTuple( dot(handMgrip, RHToScrewMatrix)) robot.mTasks['screw'].feature.selec.value = '000111' # TASK Screw orientation robot.mTasks['screw'].featureVec = FeatureVector3("featureVecScrew") plug(robot.mTasks['screw'].opPointModif.position, robot.mTasks['screw'].featureVec.signal('position')) plug(robot.mTasks['screw'].opPointModif.jacobian, robot.mTasks['screw'].featureVec.signal('Jq')) robot.mTasks['screw'].featureVec.vector.value = array([0., 0., 1.]) robot.mTasks['screw'].task.add(robot.mTasks['screw'].featureVec.name)
def get_2ht(robot, solver, TwoHandTool, gainMax, gainMin): #TwoHandTool = (0.4,-0.1,0.9,0.,0.,pi/2) #TwoHandTool = (xd,yd,zd,roll,pitch,yaw) # Homogeneous Matrix of the TwoHandTool. Normally given from the camera refToTwoHandToolMatrix = RPYToMatrix(TwoHandTool) # Homogeneous Matrixes refToTriggerMatrix = dot(refToTwoHandToolMatrix, TwoHandToolToTriggerMatrix) refToSupportMatrix = dot(refToTwoHandToolMatrix, TwoHandToolToSupportMatrix) # ---- TASKS DEFINITION ------------------------------------------------------------------- # Set the targets. Selec is the activation flag (say control only # the XYZ translation), and gain is the adaptive gain (<arg1> at the target, <arg2> # far from it, with slope st. at <arg3>m from the target, <arg4>% of the max gain # value is reached target = vectorToTuple(refToSupportMatrix[0:3, 3]) gotoNd(robot.mTasks['rh'], target, "000111", (gainMax, gainMin, 0.01, 0.9)) target = vectorToTuple(refToTriggerMatrix[0:3, 3]) gotoNd(robot.mTasks['lh'], target, "000111", (gainMax, gainMin, 0.01, 0.9)) # Orientation RF and LF - Needed featureVector3 to get desired behaviour if not hasattr(robot.mTasks['rh'], 'featureVec'): createFeatureVec(robot, 'rh', array([1., 0., 0.])) if not hasattr(robot.mTasks['lh'], 'featureVec'): createFeatureVec(robot, 'lh', array([1., 0., 0.])) robot.mTasks['rh'].featureVec.positionRef.value = dot( refToTwoHandToolMatrix[0:3, 0:3], array([1., 0., 0.])) robot.mTasks['lh'].featureVec.positionRef.value = dot( refToTwoHandToolMatrix[0:3, 0:3], array([-1., 0., 0.])) tasks = array([robot.mTasks['rh'].task, robot.mTasks['lh'].task]) # sot loading solver.sot.damping.value = 0.001 removeUndesiredTasks(solver) for i in range(len(tasks)): solver.push(tasks[i])
def plugObjectFix(robot): P72RPY = (0.75, -0.45, 0.85, 0., 0., 1.57) oIw = RPYToMatrix(P72RPY) robot.cameraFrameName = 'cameraBottomLeft' inv = Inverse_of_matrixHomo("inv") plug(robot.frames[robot.cameraFrameName].position, inv.sin) mult = Multiply_of_matrixHomo("mult") plug(inv.sout, mult.sin1) mult.sin2.value = oIw # The out signal is objectInWorld with simulated movement of the robot OBJECTINWORLD = Multiply_of_matrixHomo("OBJECTINWORLD") plug(mult.sout, OBJECTINWORLD.sin2) plug(robot.frames[robot.cameraFrameName].position, OBJECTINWORLD.sin1) return OBJECTINWORLD.sout
# -*- coding: utf-8 -*- # Copyright 2012, CNRS-LAAS, Florent Lamiraux import numpy as np from dynamic_graph.sot.core.matrix_util import RPYToMatrix from dynamic_graph.sot.pyrene.robot import Robot from dynamic_graph.sot.tools.se3 import R3, SE3 robot = Robot('seqplay') rpy2matrix = RPYToMatrix('rpy2matrix') m = 56.868 g = 9.81 pos = None zmp = None hip = None def convert(filename): """ Convert a seqplay file in OpenHRP format to sot-tool format """ global pos, zmp, hip openhrpPos = np.genfromtxt(filename + '.pos') openhrpZmp = np.genfromtxt(filename + '.zmp') nbConfig = len(openhrpPos) if len(openhrpZmp) != nbConfig: raise RuntimeError(filename + ".pos and " + filename + ".zmp have different lengths.") try: openhrpHip = np.genfromtxt(filename + '.hip')
# ------------------------------------------------------------------------------ # --- DATA ---------------------------------------------------------------- # ------------------------------------------------------------------------------ from dynamic_graph.sot.screwing.screwing_data import getData p72tohole = getData(0.) tool = (0.4, -0.1, 0.8, 0., 0., pi / 2) robot.device.viewer.updateElementConfig('TwoHandTool', (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))))
# ---- STOOL PARAMETERS ------------------------------------------------------------------- # ---- STOOL PARAMETERS ------------------------------------------------------------------- # TwoHandTool xd = 0.4 yd = -0.2 zd = 1. roll = 0. pitch = pi / 5 yaw = pi / 2 TwoHandTool = (xd, yd, zd, roll, pitch, yaw) robot.viewer.updateElementConfig('TwoHandTool', TwoHandTool) # Homogeneous Matrix of the TwoHandTool. Normally given from the camera refToTwoHandToolMatrix = array(RPYToMatrix(TwoHandTool)) # Homogeneous Matrixes refToTriggerMatrix = dot(refToTwoHandToolMatrix, TwoHandToolToTriggerMatrix) refToSupportMatrix = dot(refToTwoHandToolMatrix, TwoHandToolToSupportMatrix) # Homogeneous Matrix of the screwing part (rotation on z) refToScrewMatrix = dot(refToTwoHandToolMatrix, TwoHandToolToScrewMatrix) robot.viewer.updateElementConfig( 'zmp', vectorToTuple(refToScrewMatrix[0:3, 3]) + (0, 0, 0)) #----------------------------------------------------------------------------- # --- RUN -------------------------------------------------------------------- #-----------------------------------------------------------------------------