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 --------------------------------------------------------------------
#-----------------------------------------------------------------------------