def initDisplay(self, withDisplay=True):
if withDisplay:
from display import Display
self.viewer = Display()
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
colorred = [1.0, 0.0, 0.0, 1.0]
try:
self.viewer.viewer.gui.addCapsule('world/bcbody',
self.length * .1,
1.05 * self.length, color)
except:
pass
self.visuals.append(
Visual('world/bcbody', 1,
se3.SE3(rotate('y', np.pi / 2), zero(3))))
try:
self.viewer.viewer.gui.addCylinder('world/bcmotor1',
self.length * 0.1,
self.length * .2, colorred)
except:
pass
self.visuals.append(
Visual(
'world/bcmotor1', 1,
se3.SE3(
eye(3),
np.matrix([self.length / 2., 0, self.length * .1]).T)))
try:
self.viewer.viewer.gui.addCylinder('world/bcmotor2',
self.length * 0.1,
self.length * .2, colorred)
except:
pass
self.visuals.append(
Visual(
'world/bcmotor2', 1,
se3.SE3(
eye(3),
np.matrix([-self.length / 2., 0,
self.length * .1]).T)))
try:
self.viewer.viewer.gui.addCylinder('world/bcprop1',
self.length / 3,
self.length / 50, color)
except:
pass
self.visuals.append(
Visual(
'world/bcprop1', 1,
se3.SE3(
eye(3),
np.matrix([self.length / 2., 0, self.length * .2]).T)))
try:
self.viewer.viewer.gui.addCylinder('world/bcprop2',
self.length / 3,
self.length / 50, color)
except:
pass
self.visuals.append(
Visual(
'world/bcprop2', 1,
se3.SE3(
eye(3),
np.matrix([-self.length / 2., 0,
self.length * .2]).T)))
else:
self.viewer = None
def createJumpingProblem(self,
x0,
jumpHeight,
jumpLength,
timeStep,
groundKnots,
flyingKnots,
final=False):
q0 = x0[:self.rmodel.nq]
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
rfFootPos0 = self.rdata.oMf[self.rfId].translation
lfFootPos0 = self.rdata.oMf[self.lfId].translation
df = jumpLength[2] - rfFootPos0[2]
rfFootPos0[2] = 0.
lfFootPos0[2] = 0.
comRef = (rfFootPos0 + lfFootPos0) / 2
comRef[2] = np.asscalar(
pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
self.rWeight = 1e1
loco3dModel = []
takeOff = [
self.createSwingFootModel(
timeStep,
[self.lfId, self.rfId],
) for k in range(groundKnots)
]
flyingUpPhase = [
self.createSwingFootModel(
timeStep, [],
np.matrix([
jumpLength[0], jumpLength[1], jumpLength[2] + jumpHeight
]).T * (k + 1) / flyingKnots + comRef)
for k in range(flyingKnots)
]
flyingDownPhase = []
for k in range(flyingKnots):
flyingDownPhase += [self.createSwingFootModel(timeStep, [])]
f0 = np.matrix(jumpLength).T
footTask = [
crocoddyl.FramePlacement(self.lfId,
pinocchio.SE3(np.eye(3), self.lfId + f0)),
crocoddyl.FramePlacement(self.rfId,
pinocchio.SE3(np.eye(3), self.rfId + f0))
]
landingPhase = [
self.createFootSwitchModel([self.lfId, self.rfId], footTask, False)
]
f0[2] = df
if final is True:
self.rWeight = 1e4
landed = [
self.createSwingFootModel(timeStep, [self.lfId, self.rfId],
comTask=comRef + f0)
for k in range(groundKnots)
]
loco3dModel += takeOff
loco3dModel += flyingUpPhase
loco3dModel += flyingDownPhase
loco3dModel += landingPhase
loco3dModel += landed
problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
return problem
def display(self, x):
x = x.flat
M = se3.SE3(rotate('y', x[2]), np.matrix([x[0], 0., x[1]]).T)
for v in self.visuals:
v.place(self.viewer, M)
self.viewer.viewer.gui.refresh()
tau_f = np.array([[0., 0., 0., 0.], [0., 0., 0., 0.], [1., 1., 1., 1.], [0., d_cog, 0., -d_cog],
[-d_cog, 0., d_cog, 0.], [-cm / cf, cm / cf, -cm / cf, cm / cf]])
actuation = crocoddyl.ActuationModelMultiCopterBase(state, tau_f)
nu = actuation.nu
runningCostModel = crocoddyl.CostModelSum(state, nu)
terminalCostModel = crocoddyl.CostModelSum(state, nu)
# Costs
xResidual = crocoddyl.ResidualModelState(state, state.zero(), nu)
xActivation = crocoddyl.ActivationModelWeightedQuad(np.array([0.1] * 3 + [1000.] * 3 + [1000.] * robot_model.nv))
uResidual = crocoddyl.ResidualModelControl(state, nu)
xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
uRegCost = crocoddyl.CostModelResidual(state, uResidual)
goalTrackingResidual = crocoddyl.ResidualModelFramePlacement(state, robot_model.getFrameId("base_link"),
pinocchio.SE3(target_quat.matrix(), target_pos), nu)
goalTrackingCost = crocoddyl.CostModelResidual(state, goalTrackingResidual)
runningCostModel.addCost("xReg", xRegCost, 1e-6)
runningCostModel.addCost("uReg", uRegCost, 1e-6)
runningCostModel.addCost("trackPose", goalTrackingCost, 1e-2)
terminalCostModel.addCost("goalPose", goalTrackingCost, 3.)
dt = 3e-2
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, terminalCostModel), dt)
runningModel.u_lb = np.array([l_lim, l_lim, l_lim, l_lim])
runningModel.u_ub = np.array([u_lim, u_lim, u_lim, u_lim])
# Creating the shooting problem and the BoxDDP solver
def createFootstepModels(self, comPos0, feetPos0, stepLength, stepHeight,
timeStep, numKnots, supportFootIds, swingFootIds):
""" Action models for a footstep phase.
:param comPos0, initial CoM position
:param feetPos0: initial position of the swinging feet
:param stepLength: step length
:param stepHeight: step height
:param timeStep: time step
:param numKnots: number of knots for the footstep phase
:param supportFootIds: Ids of the supporting feet
:param swingFootIds: Ids of the swinging foot
:return footstep action models
"""
numLegs = len(supportFootIds) + len(swingFootIds)
comPercentage = float(len(swingFootIds)) / numLegs
# Action models for the foot swing
footSwingModel = []
for k in range(numKnots):
swingFootTask = []
for i, p in zip(swingFootIds, feetPos0):
# Defining a foot swing task given the step length. The swing task
# is decomposed on two phases: swing-up and swing-down. We decide
# deliveratively to allocated the same number of nodes (i.e. phKnots)
# in each phase. With this, we define a proper z-component for the
# swing-leg motion.
phKnots = numKnots / 2
if k < phKnots:
dp = np.matrix([[
stepLength * (k + 1) / numKnots, 0.,
stepHeight * k / phKnots
]]).T
elif k == phKnots:
dp = np.matrix(
[[stepLength * (k + 1) / numKnots, 0., stepHeight]]).T
else:
dp = np.matrix([[
stepLength * (k + 1) / numKnots, 0.,
stepHeight * (1 - float(k - phKnots) / phKnots)
]]).T
tref = np.asmatrix(p + dp)
swingFootTask += [
crocoddyl.FramePlacement(i, pinocchio.SE3(np.eye(3), tref))
]
comTask = np.matrix([stepLength * (k + 1) / numKnots, 0., 0.
]).T * comPercentage + comPos0
footSwingModel += [
self.createSwingFootModel(timeStep,
supportFootIds,
comTask=comTask,
swingFootTask=swingFootTask)
]
# Action model for the foot switch
footSwitchModel = self.createFootSwitchModel(supportFootIds,
swingFootTask)
# Updating the current foot position for next step
comPos0 += np.matrix([stepLength * comPercentage, 0., 0.]).T
for p in feetPos0:
p += np.matrix([[stepLength, 0., 0.]]).T
return footSwingModel + [footSwitchModel]
def rotate_J(self, jac, index):
world_R_joint = se3.SE3(self.data.oMf[index].rotation, zero(3))
return world_R_joint.action.dot(jac)
stateWeights = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) +
[10] * state.nv)
stateWeightsTerm = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) +
[100] * state.nv)
xRegCost = crocoddyl.CostModelState(
state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2),
rmodel.defaultState, actuation.nu)
uRegCost = crocoddyl.CostModelControl(state, actuation.nu)
xRegTermCost = crocoddyl.CostModelState(
state, crocoddyl.ActivationModelWeightedQuad(stateWeightsTerm**2),
rmodel.defaultState, actuation.nu)
# Cost for target reaching: hand and foot
handTrackingWeights = np.array([1] * 3 + [0.0001] * 3)
Pref = crocoddyl.FramePlacement(endEffectorId,
pinocchio.SE3(np.eye(3), target))
handTrackingCost = crocoddyl.CostModelFramePlacement(
state, crocoddyl.ActivationModelWeightedQuad(handTrackingWeights**2), Pref,
actuation.nu)
footTrackingWeights = np.array([1, 1, 0.1] + [1.] * 3)
Pref = crocoddyl.FramePlacement(
leftFootId, pinocchio.SE3(np.eye(3), np.array([0., 0.4, 0.])))
footTrackingCost1 = crocoddyl.CostModelFramePlacement(
state, crocoddyl.ActivationModelWeightedQuad(footTrackingWeights**2), Pref,
actuation.nu)
Pref = crocoddyl.FramePlacement(
leftFootId, pinocchio.SE3(np.eye(3), np.array([0.3, 0.15, 0.35])))
footTrackingCost2 = crocoddyl.CostModelFramePlacement(
state, crocoddyl.ActivationModelWeightedQuad(footTrackingWeights**2), Pref,
actuation.nu)
from __future__ import print_function
import numpy as np
from numpy.linalg import norm, solve
import pinocchio
model = pinocchio.buildSampleModelManipulator()
data = model.createData()
JOINT_ID = 6
oMdes = pinocchio.SE3(np.eye(3), np.array([1., 0., 1.]))
q = pinocchio.neutral(model)
eps = 1e-4
IT_MAX = 1000
DT = 1e-1
damp = 1e-6
i = 0
while True:
pinocchio.forwardKinematics(model, data, q)
dMi = oMdes.actInv(data.oMi[JOINT_ID])
err = pinocchio.log(dMi).vector
if norm(err) < eps:
success = True
break
if i >= IT_MAX:
success = False
break
J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID)
def check_pyb_env(self, k, envID, velID, qmes12):
"""Check the state of the robot to trigger events and update camera
Args:
k (int): Number of inv dynamics iterations since the start of the simulation
envID (int): Identifier of the current environment to be able to handle different scenarios
velID (int): Identifier of the current velocity profile to be able to handle different scenarios
qmes12 (19x1 array): the position/orientation of the trunk and angular position of actuators
"""
# If spheres are loaded
if envID == 1:
# Check if the robot is in front of the first sphere to trigger it
if self.flag_sphere1 and (qmes12[1, 0] >= 0.9):
pyb.resetBaseVelocity(
self.sphereId1, linearVelocity=[2.5, 0.0, 2.0])
self.flag_sphere1 = False
# Check if the robot is in front of the second sphere to trigger it
if self.flag_sphere2 and (qmes12[1, 0] >= 1.1):
pyb.resetBaseVelocity(
self.sphereId2, linearVelocity=[-2.5, 0.0, 2.0])
self.flag_sphere2 = False
# Create the red steps to act as small perturbations
"""if k == 10:
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
mesh_scale = [2.0, 2.0, 0.3]
visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
fileName="cube.obj",
halfExtents=[0.5, 0.5, 0.1],
rgbaColor=[0.0, 0.0, 1.0, 1.0],
specularColor=[0.4, .4, 0],
visualFramePosition=[0.0, 0.0, 0.0],
meshScale=mesh_scale)
collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH,
fileName="cube.obj",
collisionFramePosition=[0.0, 0.0, 0.0],
meshScale=mesh_scale)
tmpId = pyb.createMultiBody(baseMass=0.0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0.0, 0.0, 0.15],
useMaximalCoordinates=True)
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
pyb.resetBasePositionAndOrientation(self.robotId, [0, 0, 0.5], [0, 0, 0, 1])"""
if k == 10:
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
mesh_scale = [0.1, 0.1, 0.04]
visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
fileName="cube.obj",
halfExtents=[
0.5, 0.5, 0.1],
rgbaColor=[
0.0, 0.0, 1.0, 1.0],
specularColor=[
0.4, .4, 0],
visualFramePosition=[
0.0, 0.0, 0.0],
meshScale=mesh_scale)
collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH,
fileName="cube.obj",
collisionFramePosition=[
0.0, 0.0, 0.0],
meshScale=mesh_scale)
tmpId = pyb.createMultiBody(baseMass=0.0,
baseInertialFramePosition=[
0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0.19, 0.15005, 0.02],
useMaximalCoordinates=True)
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
pyb.resetBasePositionAndOrientation(
self.robotId, [0, 0, 0.25], [0, 0, 0, 1])
# Apply perturbations by directly applying external forces on the robot trunk
if velID == 4:
self.apply_external_force(k, 4250, 500, np.array(
[0.0, 0.0, -3.0]), np.zeros((3,)))
self.apply_external_force(k, 5250, 500, np.array(
[0.0, +3.0, 0.0]), np.zeros((3,)))
"""if velID == 0:
self.apply_external_force(k, 1000, 1000, np.array([0.0, 0.0, -6.0]), np.zeros((3,)))
self.apply_external_force(k, 2000, 1000, np.array([0.0, +12.0, 0.0]), np.zeros((3,)))"""
# Update the PyBullet camera on the robot position to do as if it was attached to the robot
"""pyb.resetDebugVisualizerCamera(cameraDistance=0.75, cameraYaw=+50, cameraPitch=-35,
cameraTargetPosition=[qmes12[0, 0], qmes12[1, 0] + 0.0, 0.0])"""
# Get the orientation of the robot to change the orientation of the camera with the rotation of the robot
oMb_tmp = pin.SE3(pin.Quaternion(
qmes12[3:7]), np.array([0.0, 0.0, 0.0]))
RPY = pin.rpy.matrixToRpy(oMb_tmp.rotation)
# Update the PyBullet camera on the robot position to do as if it was attached to the robot
pyb.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=45, cameraPitch=-39.9,
cameraTargetPosition=[0.0, 0.0, 0.0])
return 0
# Cost for state and control
xResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu)
xActivation = crocoddyl.ActivationModelWeightedQuad(
np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) +
[10] * state.nv)**2)
uResidual = crocoddyl.ResidualModelControl(state, actuation.nu)
xTActivation = crocoddyl.ActivationModelWeightedQuad(
np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) +
[100] * state.nv)**2)
xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
uRegCost = crocoddyl.CostModelResidual(state, uResidual)
xRegTermCost = crocoddyl.CostModelResidual(state, xTActivation, xResidual)
# Cost for target reaching
framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
state, endEffectorId, pinocchio.SE3(np.eye(3), target), actuation.nu)
framePlacementActivation = crocoddyl.ActivationModelWeightedQuad(
np.array([1] * 3 + [0.0001] * 3)**2)
goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementActivation,
framePlacementResidual)
# Cost for CoM reference
comResidual = crocoddyl.ResidualModelCoMPosition(state, comRef, actuation.nu)
comTrack = crocoddyl.CostModelResidual(state, comResidual)
# Create cost model per each action model
runningCostModel = crocoddyl.CostModelSum(state, actuation.nu)
terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu)
# Then let's added the running and terminal cost functions
runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
def createHand(self,rootId=0,jointPlacement=None):
color = [red,green,blue,transparency] = [1,1,0.78,1.0]
colorred = [1.0,0.0,0.0,1.0]
jointId = rootId
cm = 1e-2
trans = lambda x,y,z: pio.SE3(eye(3),np.matrix([x,y,z]).T)
inertia = lambda m,c: pio.Inertia(m,np.matrix(c,np.double).T,eye(3)*m**2)
name = "wrist"
jointName,bodyName = [name+"_joint",name+"_body"]
#jointPlacement = jointPlacement if jointPlacement!=None else pio.SE3.Identity()
jointPlacement = jointPlacement if jointPlacement!=None else pio.SE3(pio.utils.rotate('y',np.pi),zero(3))
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(3,[0,0,0]),pio.SE3.Identity())
## Hand dimsensions: length, width, height(=depth), finger-length
L=3*cm;W=5*cm;H=1*cm; FL = 4*cm
self.addCapsule('world/wrist',jointId,
pio.SE3(rotate('x',pi/2),np.matrix([0,0,0]).T),.02,0 )
self.addCapsule('world/wpalml',jointId,
pio.SE3(rotate('z',-.3)*rotate('y',pi/2),np.matrix([L/2,-W/2.6,0]).T),H,L )
#pio.SE3(rotate('y',pi/2),np.matrix([L/2,-W/2,0]).T),H,L )
self.addCapsule('world/wpalmr',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([L/2,W/2,0]).T),H,L)
self.addCapsule('world/wpalmfr',jointId,
pio.SE3(rotate('x',pi/2),np.matrix([L,0,0]).T),H,W)
name = "palm"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([5*cm,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(2,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/palm2',jointId,
# pio.SE3(rotate('y',pi/2),zero(3)),1*cm,W )
pio.SE3(rotate('x',pi/2),zero(3)),1*cm,W )
palmIdx = jointId
name = "finger11"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([2*cm,W/2,0]).T)
jointId = self.model.addJoint(palmIdx,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger11',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H )
name = "finger12"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger12',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H )
name = "finger13"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL-2*H,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.3,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger13',jointId,
trans(2*H,0,0),H,0 )
name = "finger21"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([2*cm,0,0]).T)
jointId = self.model.addJoint(palmIdx,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger21',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H )
name = "finger22"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger22',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H )
name = "finger23"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL-H,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.3,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger23',jointId,
trans(H,0,0),H,0 )
name = "finger31"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([2*cm,-W/2,0]).T)
jointId = self.model.addJoint(palmIdx,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger31',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H)
name = "finger32"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger32',jointId,
pio.SE3(rotate('y',pi/2),np.matrix([FL/2-H,0,0]).T),H,FL-2*H)
name = "finger33"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), np.matrix([FL-2*H,0,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.3,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/finger33',jointId,
trans(2*H,0,0),H,0)
name = "thumb1"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(rotate('z',-1), np.matrix([1*cm,-W/2-H*1.3,0]).T)
jointId = self.model.addJoint(1,pio.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
# self.addCapsule('world/thumb1',jointId,
# pio.SE3(rotate('z',pi/3)*rotate('x',pi/2),np.matrix([1*cm,-1*cm,0]).T),
# H,2*cm)
name = "thumb1a"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(eye(3), zero(3))
jointId = self.model.addJoint(jointId,pio.JointModelRX(),jointPlacement,jointName)
# self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/thumb1',jointId,
pio.SE3(rotate('z',pi/3)*rotate('x',pi/2),np.matrix([0.3*cm,-1.0*cm,0]).T),
H,2*cm)
name = "thumb2"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pio.SE3(rotate('z',pi/3)*rotate('x',pi), np.matrix([3*cm,-1.8*cm,0]).T)
jointId = self.model.addJoint(jointId,pio.JointModelRZ(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.4,[0,0,0]),pio.SE3.Identity())
self.addCapsule('world/thumb2',jointId,
pio.SE3(rotate('x',pi/3),np.matrix([-0.7*cm,.8*cm,-0.5*cm]).T),
H,FL-2*H)
# Prepare some patches to represent collision points. Yet unvisible.
if self.viewer is not None:
self.maxContact = 10
for i in range(self.maxContact):
self.viewer.addCylinder('world/cpatch%d'%i, .01, .003, [ 1.0,0,0,1])
self.viewer.setVisibility('world/cpatch%d'%i,'OFF')
def xyzrpyToSE3(xyzrpy):
r = se3.utils.rpyToMatrix(np.matrix(xyzrpy[3:6]).T)
return se3.SE3(r, np.matrix(xyzrpy[:3]))
def homo16toSE3(M):
r = np.matrix([[M[0], M[1], M[2]], [M[4], M[5], M[6]], [M[8], M[9],
M[10]]])
t = np.matrix([[M[3]], [M[7]], [M[11]]])
return se3.SE3(r, t)
def se3Interp(s1, s2, alpha):
t = (s1.translation * alpha + s2.translation * (1 - alpha))
r = se3.exp3(
se3.log3(s1.rotation) * alpha + se3.log3(s2.rotation) * (1 - alpha))
return se3.SE3(r, t)
class CallbackLogger:
def __init__(self, sleeptime=1e-2):
"""
nfevl: iteration number
dt: animation time pause
"""
self.nfeval = 1
self.dt = sleeptime
def __call__(self, q):
print('===CBK=== {0:4d} {1: 3.2f} {2: 3.2f}'.format(
self.nfeval, q[0], q[1]))
robot.display(q)
place('world/blue', robot.position(q, 6))
self.nfeval += 1
time.sleep(self.dt)
# Question 1-2-3
target = np.matrix([0.5, 0.1, 0.2]).T # x,y,z
place('world/red', pinocchio.SE3(eye(3), target))
cost = Cost(target)
robot.display(robot.q0)
time.sleep(1)
print('Let s go to pdes.')
def go():
qopt = fmin_bfgs(cost, robot.q0, callback=CallbackLogger(.5))
return qopt
state = crocoddyl.StateMultibody(robot_model)
distance_rotor_COG = 0.1525
cf = 6.6e-5
cm = 1e-6
u_lim = 5
l_lim = 0.1
actModel = ActuationModelUAM(state, '+', distance_rotor_COG, cm, cf, u_lim,
l_lim)
runningCostModel = crocoddyl.CostModelSum(state, actModel.nu)
terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu)
# Needed objects to create the costs
Mref = crocoddyl.FramePlacement(
robot_model.getFrameId("base_link"),
pinocchio.SE3(target_quat.matrix(),
np.matrix(target_pos).T))
wBasePos, wBaseOri, wBaseVel, wBaseRate = [0.1], [1000], [1000], [10]
stateWeights = np.matrix(
[wBasePos * 3 + wBaseOri * 3 + wBaseVel * robot_model.nv]).T
# Costs
goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref, actModel.nu)
xRegCost = crocoddyl.CostModelState(
state, crocoddyl.ActivationModelWeightedQuad(stateWeights), state.zero(),
actModel.nu)
uRegCost = crocoddyl.CostModelControl(state, actModel.nu)
runningCostModel.addCost("xReg", xRegCost, 1e-6)
runningCostModel.addCost("uReg", uRegCost, 1e-6)
runningCostModel.addCost("trackPose", goalTrackingCost, 1e-2)
terminalCostModel.addCost("goalPose", goalTrackingCost, 100)
