Commit 4d617873 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

[tp3] ivigit.

parent f7509b36
This diff is collapsed.
'''
Inverse kinematics (close loop / iterative) for a mobile manipulator.
Template of the program for TP3
'''
import pinocchio as pin
import numpy as np
import time
from numpy.linalg import pinv,inv,norm,svd,eig
from tiago_loader import loadTiago
import matplotlib.pylab as plt; plt.ion()
from tp3.meshcat_viewer_wrapper import MeshcatVisualizer
robot = loadTiago(addGazeFrame=True)
viz = MeshcatVisualizer(robot,url='classical')
NQ = robot.model.nq
NV = robot.model.nv
IDX_TOOL = robot.model.getFrameId('frametool')
IDX_BASIS = robot.model.getFrameId('framebasis')
IDX_GAZE = robot.model.getFrameId('framegaze')
# Add a small ball as a visual target to be reached by the robot
ball = np.array([ 1.2,0.5,1.1 ])
viz.addSphere('ball', .05, [ .8,.1,.5, .8] )
viz.applyConfiguration('ball', list(ball)+[0,0,0,1])
# Goal placement, and integration in the viewer of the goal.
oMgoal = pin.SE3(pin.Quaternion(-0.5, 0.58, -0.39, 0.52).normalized().matrix(),
np.array([1.2, .4, .7]))
viz.addBox('goal', [.1,.1,.1], [ .1,.1,.5, .6] )
viz.applyConfiguration('goal',oMgoal)
# Integration step.
DT = 1e-2
# Robot initial configuration.
q0 = np.array([ 0. , 0. , 0. , 1. , 0.18, 1.37, -0.24, -0.98, 0.98,
0. , 0. , 0. , 0. , -0.13, 0. , 0. , 0. , 0. ])
q = q0.copy()
herr = [] # Log the value of the error between gaze and ball.
# Loop on an inverse kinematics for 200 iterations.
for i in range(200): # Integrate over 2 second of robot life
# Run the algorithms that outputs values in robot.data
pin.framesForwardKinematics(robot.model,robot.data,q)
pin.computeJointJacobians(robot.model,robot.data,q)
# Placement from world frame o to frame f oMgaze
oMgaze = robot.data.oMf[IDX_GAZE]
# 6D jacobian in local frame
o_Jgaze3 = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_GAZE,pin.LOCAL_WORLD_ALIGNED)[:3,:]
# vector from gaze to ball, in world frame
o_GazeBall = oMgaze.translation-ball
vq = -pinv(o_Jgaze3) @ o_GazeBall
q = pin.integrate(robot.model,q, vq * DT)
viz.display(q)
time.sleep(1e-3)
herr.append(o_GazeBall)
q = q0.copy()
herr = [] # Log the value of the error between tool and goal.
herr2 = [] # Log the value of the error between gaze and ball.
# Loop on an inverse kinematics for 200 iterations.
for i in range(200): # Integrate over 2 second of robot life
pin.framesForwardKinematics(robot.model,robot.data,q)
pin.computeJointJacobians(robot.model,robot.data,q)
# Gaze task
oMgaze = robot.data.oMf[IDX_GAZE]
o_Jgaze3 = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_GAZE,pin.LOCAL_WORLD_ALIGNED)[:3,:]
o_GazeBall = oMgaze.translation-ball
# Tool task
oMtool = robot.data.oMf[IDX_TOOL]
o_Jtool3 = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL,pin.LOCAL_WORLD_ALIGNED)[:3,:]
o_TG = oMtool.translation-oMgoal.translation
vq = -pinv(o_Jtool3) @ o_TG
Ptool = np.eye(robot.nv)-pinv(o_Jtool3) @ o_Jtool3
vq += pinv(o_Jgaze3 @ Ptool) @ (-o_GazeBall - o_Jgaze3 @ vq)
q = pin.integrate(robot.model,q, vq * DT)
viz.display(q)
time.sleep(1e-3)
herr.append(o_TG)
herr2.append(o_GazeBall)
'''
Inverse kinematics (close loop / iterative) for a mobile manipulator.
Template of the program for TP3
'''
import pinocchio as pin
import numpy as np
import time
from numpy.linalg import pinv,inv,norm,svd,eig
from tiago_loader import loadTiago
import matplotlib.pylab as plt; plt.ion()
from tp3.meshcat_viewer_wrapper import MeshcatVisualizer
robot = loadTiago()
viz = MeshcatVisualizer(robot,url='classical')
NQ = robot.model.nq
NV = robot.model.nv
IDX_TOOL = robot.model.getFrameId('frametool')
IDX_BASIS = robot.model.getFrameId('framebasis')
IDX_GAZE = robot.model.getFrameId('framegaze')
# Goal placement, and integration in the viewer of the goal.
oMgoal = pin.SE3(pin.Quaternion(-0.5, 0.58, -0.39, 0.52).normalized().matrix(),
np.array([1.2, .4, .7]))
viz.addBox('goal', [.1,.1,.1], [ .1,.1,.5, .6] )
viz.applyConfiguration('goal',oMgoal)
# Integration step.
DT = 1e-2
# Robot initial configuration.
q0 = np.array([ 0. , 0. , 1. , 0. , 0.18, 1.37, -0.24, -0.98, 0.98,
0. , 0. , 0. , 0. , -0.13, 0. , 0. , 0. , 0. ])
q = q0.copy()
herr = [] # Log the value of the error between tool and goal.
# Loop on an inverse kinematics for 200 iterations.
for i in range(200): # Integrate over 2 second of robot life
# Run the algorithms that outputs values in robot.data
pin.framesForwardKinematics(robot.model,robot.data,q)
pin.computeJointJacobians(robot.model,robot.data,q)
# Placement from world frame o to frame f oMtool
oMtool = robot.data.oMf[IDX_TOOL]
# 3D jacobian in world frame
o_Jtool3 = pin.computeFrameJacobian(robot.model,robot.data,q,IDX_TOOL,pin.LOCAL_WORLD_ALIGNED)[:3,:]
# vector from tool to goal, in world frame
o_TG = oMtool.translation-oMgoal.translation
# Control law by least square
vq = -pinv(o_Jtool3)@o_TG
q = pin.integrate(robot.model,q, vq * DT)
viz.display(q)
time.sleep(1e-3)
herr.append(o_TG)
q = q0.copy()
herr = []
for i in range(200): # Integrate over 2 second of robot life
# Run the algorithms that outputs values in robot.data
pin.framesForwardKinematics(robot.model,robot.data,q)
pin.computeJointJacobians(robot.model,robot.data,q)
# Placement from world frame o to frame f oMtool
oMtool = robot.data.oMf[IDX_TOOL]
# 6D error between the two frame
tool_nu = pin.log(oMtool.inverse()*oMgoal).vector
# Get corresponding jacobian
tool_Jtool = pin.computeFrameJacobian(robot.model, robot.data, q, IDX_TOOL)
# Control law by least square
vq = pinv(tool_Jtool)@tool_nu
q = pin.integrate(robot.model,q, vq * DT)
viz.display(q)
time.sleep(1e-3)
herr.append(tool_nu)
../tp2/meshcat_viewer_wrapper
\ No newline at end of file
import numpy as np
import pinocchio as pin
import example_robot_data as robex
import hppfcl
from os.path import dirname, exists, join
class TiagoLoader(object):
#path = ''
#urdf_filename = ''
srdf_filename = ''
urdf_subpath = 'robots'
srdf_subpath = 'srdf'
ref_posture = 'half_sitting'
has_rotor_parameters = False
free_flyer = True
verbose = False
path = "tiago_description"
urdf_filename = "tiago_no_hand.urdf"
def __init__(self):
urdf_path = join(self.path, self.urdf_subpath, self.urdf_filename)
self.model_path = robex.getModelPath(urdf_path, self.verbose)
self.urdf_path = join(self.model_path, urdf_path)
self.robot = pin.RobotWrapper.BuildFromURDF(self.urdf_path, [join(self.model_path, '../..')],
pin.JointModelPlanar() if self.free_flyer else None)
if self.srdf_filename:
self.srdf_path = join(self.model_path, self.path, self.srdf_subpath, self.srdf_filename)
self.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose, self.has_rotor_parameters,
self.ref_posture)
else:
self.srdf_path = None
self.q0 = None
if self.free_flyer:
self.addFreeFlyerJointLimits()
def addFreeFlyerJointLimits(self):
ub = self.robot.model.upperPositionLimit
ub[:self.robot.model.joints[1].nq] = 1
self.robot.model.upperPositionLimit = ub
lb = self.robot.model.lowerPositionLimit
lb[:self.robot.model.joints[1].nq] = -1
self.robot.model.lowerPositionLimit = lb
def loadTiago(addGazeFrame=False):
'''
Load a tiago model, without the hand, and with the two following modifications wrt example_robot_data.
- first, the first joint is a planar (x,y,cos,sin) joint, while it is a fixed robot in example robot data.
- second, two visual models of a frame have been added to two new op-frame, "tool0" on the robot hand, and "basis0" in
front of the basis.
'''
robot = TiagoLoader().robot
geom = robot.visual_model
X = pin.utils.rotate('y', np.pi/2)
Y = pin.utils.rotate('x',-np.pi/2)
Z = np.eye(3)
L = .3
cyl=hppfcl.Cylinder(L/30,L)
med = np.array([0,0,L/2])
# ---------------------------------------------------------------------------
# Add a frame visualisation in the effector.
FIDX = robot.model.getFrameId('wrist_ft_tool_link')
JIDX = robot.model.frames[FIDX].parent
eff = np.array([0,0,.08])
FIDX = robot.model.addFrame(pin.Frame('frametool',JIDX,FIDX,pin.SE3(Z,eff),pin.FrameType.OP_FRAME))
geom.addGeometryObject(pin.GeometryObject('axis_x',FIDX,JIDX,cyl,pin.SE3(X,X@med+eff)))
geom.geometryObjects[-1].meshColor = np.array([1,0,0,1.])
geom.addGeometryObject(pin.GeometryObject('axis_y',FIDX,JIDX,cyl,pin.SE3(Y,Y@med+eff)))
geom.geometryObjects[-1].meshColor = np.array([0,1,0,1.])
geom.addGeometryObject(pin.GeometryObject('axis_z',FIDX,JIDX,cyl,pin.SE3(Z,Z@med+eff)))
geom.geometryObjects[-1].meshColor = np.array([0,0,1,1.])
# ---------------------------------------------------------------------------
# Add a frame visualisation in front of the basis.
FIDX = robot.model.getFrameId('base_link')
JIDX = robot.model.frames[FIDX].parent
eff = np.array([.3,0,.15])
FIDX = robot.model.addFrame(pin.Frame('framebasis',JIDX,FIDX,pin.SE3(Z,eff),pin.FrameType.OP_FRAME))
geom.addGeometryObject(pin.GeometryObject('axis2_x',FIDX,JIDX,cyl,pin.SE3(X,X@med+eff)))
geom.geometryObjects[-1].meshColor = np.array([1,0,0,1.])
geom.addGeometryObject(pin.GeometryObject('axi2_y',FIDX,JIDX,cyl,pin.SE3(Y,Y@med+eff)))
geom.geometryObjects[-1].meshColor = np.array([0,1,0,1.])
geom.addGeometryObject(pin.GeometryObject('axis2_z',FIDX,JIDX,cyl,pin.SE3(Z,Z@med+eff)))
geom.geometryObjects[-1].meshColor = np.array([0,0,1,1.])
# ---------------------------------------------------------------------------
# Add a frame visualisation in front of the head.
if addGazeFrame:
L = .05
cyl=hppfcl.Cylinder(L/30,L)
med = np.array([0,0,L/2])
FIDX = robot.model.getFrameId('xtion_joint')
JIDX = robot.model.frames[FIDX].parent
eff = np.array([0.4,0.0,0.0])
FIDX = robot.model.addFrame(pin.Frame('framegaze',JIDX,FIDX,pin.SE3(Z,eff),pin.FrameType.OP_FRAME))
geom.addGeometryObject(pin.GeometryObject('axisgaze_x',FIDX,JIDX,cyl,pin.SE3(X,X@med+eff)))
geom.geometryObjects[-1].meshColor = np.array([1,0,0,1.])
geom.addGeometryObject(pin.GeometryObject('axisgaze_y',FIDX,JIDX,cyl,pin.SE3(Y,Y@med+eff)))
geom.geometryObjects[-1].meshColor = np.array([0,1,0,1.])
geom.addGeometryObject(pin.GeometryObject('axisgaze_z',FIDX,JIDX,cyl,pin.SE3(Z,Z@med+eff)))
geom.geometryObjects[-1].meshColor = np.array([0,0,1,1.])
# -------------------------------------------------------------------------------
# Regenerate the data from the new models.
robot.q0 = np.array([1,1,1,0]+[0]*(robot.model.nq-4))
robot.data = robot.model.createData()
robot.visual_data = robot.visual_model.createData()
return robot
# ------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------
# ------------------------------------------------------------------------------------------------
if __name__ == "__main__":
from tp2.meshcat_viewer_wrapper import MeshcatVisualizer
robot = loadTiago()
viz = MeshcatVisualizer(robot,url='classical')
viz.display(robot.q0)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment