def solve(self, M_des, robot, joint_id, q=None):
""" Inverse kinematics for specified joint (joint_id) and desired pose M_des (array 4x4)
NOTE The code below is adopted from here (01.03.2020):
https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_b-examples_i-inverse-kinematics.html
"""
oMdes = pinocchio.SE3(M_des[:3, :3], M_des[:3, 3])
if np.all(q == None): q = pinocchio.neutral(robot.model)
i = 0
iter_resample = 0
while True:
# forward
pinocchio.forwardKinematics(robot.model, robot.data, q)
# error between desired pose and the current one
dMi = oMdes.actInv(robot.data.oMi[joint_id])
err = pinocchio.log(dMi).vector
# Termination criteria
if norm(err) < self.inv_kin_sol_params.eps:
success = True
break
if i >= self.inv_kin_sol_params.max_iter:
if iter_resample <= self.inv_kin_sol_params.max_resample:
q = pinocchio.randomConfiguration(robot.model)
iter_resample += 1
continue
else:
success = False
break
# Jacobian
J = pinocchio.computeJointJacobian(robot.model, robot.data, q,
joint_id)
# P controller (?)
# v* =~ A * e
v = -J.T.dot(
solve(
J.dot(J.T) + self.inv_kin_sol_params.damp * np.eye(6),
err))
# integrate (in this case only sum)
q = pinocchio.integrate(robot.model, q,
v * self.inv_kin_sol_params.dt)
i += 1
if not success: q = pinocchio.neutral(robot.model)
q_arr = np.squeeze(np.array(q))
q_arr = np.mod(np.abs(q_arr), 2 * np.pi) * np.sign(q_arr)
mask = np.abs(q_arr) > np.pi
# If angle abs(q) is larger than pi, represent angle as the shorter angle inv_sign(q) * (2*pi - abs(q))
# This is to avoid conflicting with angle limits.
q_arr[mask] = (-1) * np.sign(
q_arr[mask]) * (2 * np.pi - np.abs(q_arr[mask]))
return success, q_arr
def __init__(self):
urdf_path = join(self.path, self.urdf_subpath, self.urdf_filename)
self.model_path = getModelPath(urdf_path, self.verbose)
self.urdf_path = join(self.model_path, urdf_path)
self.robot = RobotWrapper.BuildFromURDF(self.urdf_path, [join(self.model_path, '../..')],
pin.JointModelFreeFlyer() 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.robot.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose,
self.has_rotor_parameters, self.ref_posture)
if pin.WITH_HPP_FCL and pin.WITH_HPP_FCL_BINDINGS:
# Add all collision pairs
self.robot.collision_model.addAllCollisionPairs()
# Remove collision pairs per SRDF
pin.removeCollisionPairs(self.robot.model, self.robot.collision_model, self.srdf_path, False)
# Recreate collision data since the collision pairs changed
self.robot.collision_data = self.robot.collision_model.createData()
else:
self.srdf_path = None
self.robot.q0 = pin.neutral(self.robot.model)
if self.free_flyer:
self.addFreeFlyerJointLimits()
def test_std_map_fields(self):
model = self.model
model.referenceConfigurations["neutral"] = pin.neutral(model)
q_neutral = model.referenceConfigurations["neutral"]
q_neutral.fill(1.)
self.assertApprox(model.referenceConfigurations["neutral"], q_neutral)
def __init__(self):
self.viewer = Display()
self.visuals = []
self.model = Model()
self.createLeggedRobot()
self.data = self.model.createData()
self.q0 = neutral(self.model)
def test_frame_algo(self):
model = self.model
data = model.createData()
q = pin.neutral(model)
v = np.random.rand((model.nv))
frame_id = self.frame_idx
J1 = pin.computeFrameJacobian(model, data, q, frame_id)
J2 = pin.computeFrameJacobian(model, data, q, frame_id, pin.LOCAL)
self.assertApprox(J1, J2)
data2 = model.createData()
pin.computeJointJacobians(model, data2, q)
J3 = pin.getFrameJacobian(model, data2, frame_id, pin.LOCAL)
self.assertApprox(J1, J3)
dJ1 = pin.frameJacobianTimeVariation(model, data, q, v, frame_id,
pin.LOCAL)
data3 = model.createData()
pin.computeJointJacobiansTimeVariation(model, data3, q, v)
dJ2 = pin.getFrameJacobianTimeVariation(model, data3, frame_id,
pin.LOCAL)
self.assertApprox(dJ1, dJ2)
def __init__(self, robot):
self.q = neutral(robot.model)
forwardKinematics(robot.model, robot.data, self.q)
self.robot = robot
# Initialize references of feet and center of mass with initial values
self.leftFootRefPose = robot.data.oMi[robot.leftFootJointId].copy()
self.rightFootRefPose = robot.data.oMi[robot.rightFootJointId].copy()
self.waistRefPose = robot.data.oMi[robot.waistJointId].copy()
def readParamsFromSrdf(model, SRDF_PATH, verbose=False, has_rotor_parameters=True, referencePose='half_sitting'):
if has_rotor_parameters:
pin.loadRotorParameters(model, SRDF_PATH, verbose)
model.armature = np.multiply(model.rotorInertia.flat, np.square(model.rotorGearRatio.flat))
pin.loadReferenceConfigurations(model, SRDF_PATH, verbose)
q0 = pin.neutral(model)
if referencePose is not None:
q0 = model.referenceConfigurations[referencePose].copy()
return q0
def test_copy(self):
data2 = self.data.copy()
q = pin.neutral(self.model)
pin.forwardKinematics(self.model,data2,q)
jointId = self.model.njoints-1
self.assertNotEqual(self.data.oMi[jointId], data2.oMi[jointId])
data3 = data2.copy()
self.assertEqual(data2.oMi[jointId], data3.oMi[jointId])
def test_copy(self):
data2 = self.data.copy()
q = pin.neutral(self.model)
pin.forwardKinematics(self.model, data2, q)
jointId = self.model.njoints - 1
self.assertNotEqual(self.data.oMi[jointId], data2.oMi[jointId])
data3 = data2.copy()
self.assertEqual(data2.oMi[jointId], data3.oMi[jointId])
def removeJoints(self, joints):
"""
- param joints: a list of joint names to be removed from the self.pinocchioModel
"""
jointIds = list()
for j in joints:
if self.pinocchioModel.existJointName(j):
jointIds.append(self.pinocchioModel.getJointId(j))
if len(jointIds) > 0:
q = pinocchio.neutral(self.pinocchioModel)
self.pinocchioModel = pinocchio.buildReducedModel(self.pinocchioModel, jointIds, q)
self.pinocchioData = pinocchio.Data(self.pinocchioModel)
def optimize_initial_position(self, init_state):
# Optimize the initial configuration
q = se3.neutral(self.robot.model)
plan_joint_init_pos = self.planner_setting.get(
PlannerVectorParam_KinematicDefaultJointPositions)
if len(plan_joint_init_pos) != self.robot.num_ctrl_joints:
raise ValueError(
'Number of joints in config file not same as required for robot\n'
+ 'Got %d joints but robot expects %d joints.' %
(len(plan_joint_init_pos), self.robot.num_ctrl_joints))
q[7:] = np.matrix(plan_joint_init_pos).T
q[2] = self.robot.floor_height + 0.32 #was 0.32
#print q[2]
dq = np.matrix(np.zeros(self.robot.robot.nv)).T
com_ref = init_state.com
lmom_ref = np.zeros(3)
amom_ref = np.zeros(3)
endeff_pos_ref = np.array(
[init_state.effPosition(i) for i in range(init_state.effNum())])
endeff_vel_ref = np.matrix(np.zeros((init_state.effNum(), 3)))
endeff_contact = np.ones(init_state.effNum())
quad_goal = se3.Quaternion(
se3.rpy.rpyToMatrix(np.matrix([0.0, 0, 0.]).T))
q[3:7] = quad_goal.coeffs()
for iters in range(self.max_iterations):
# Adding small P controller for the base orientation to always start with flat
# oriented base.
quad_q = se3.Quaternion(float(q[6]), float(q[3]), float(q[4]),
float(q[5]))
amom_ref = 1e-1 * se3.log((quad_goal * quad_q.inverse()).matrix())
res = self.inv_kin.compute(q, dq, com_ref, lmom_ref, amom_ref,
endeff_pos_ref, endeff_vel_ref,
endeff_contact, None)
q = se3.integrate(self.robot.model, q, res)
if np.linalg.norm(res) < 1e-3:
print('Found initial configuration after {} iterations'.format(
iters + 1))
break
if iters == self.max_iterations - 1:
print('Failed to converge for initial setup.')
print("initial configuration: \n", q)
q[2] = 0.20
self.q_init = q.copy()
self.dq_init = dq.copy()
def __init__(self,
robot,
controller=None,
use_theoretical_model=False,
viewer_backend=None):
"""
@brief Constructor
@param[in] robot Jiminy robot properly setup (eg sensors already added)
@param[in] use_theoretical_model Whether the state corresponds to the theoretical model
@param[in] viewer_backend Backend of the viewer (gepetto-gui or meshcat)
@return Instance of the wrapper.
"""
# Backup the user arguments
self.robot = robot # For convenience, since the engine will manage its lifetime anyway
self.use_theoretical_model = use_theoretical_model
self.viewer_backend = viewer_backend
# Make sure that the sensors have already been added to the robot !
self._sensors_types = robot.get_sensors_options().keys()
self._t = -1
self._state = None
self._sensor_data = None
self._action = np.zeros((robot.nmotors, ))
# Instantiate the Jiminy controller if necessary, then initialize it
if controller is None:
self._controller = jiminy.ControllerFunctor(
self._send_command, self._internal_dynamics)
else:
self._controller = controller
self._controller.initialize(robot)
# Instantiate the Jiminy engine
self._engine = jiminy.Engine()
self._engine.initialize(robot, self._controller)
## Viewer management
self._viewer = None
self._is_viewer_available = False
## Real time rendering management
self.step_dt_prev = -1
# Initialize the low-level jiminy engine
q0 = neutral(robot.pinocchio_model)
v0 = np.zeros(robot.nv)
self.reset(np.concatenate((q0, v0)), is_state_theoretical=False)
def _neutral(self) -> np.ndarray:
""" TODO: Write documentation.
"""
def joint_position_idx(joint_name: str) -> int:
joint_idx = self.robot.pinocchio_model.getJointId(joint_name)
return self.robot.pinocchio_model.joints[joint_idx].idx_q
qpos = neutral(self.robot.pinocchio_model)
qpos[2] = 0.75
qpos[joint_position_idx('ankle_1')] = 1.0
qpos[joint_position_idx('ankle_2')] = -1.0
qpos[joint_position_idx('ankle_3')] = -1.0
qpos[joint_position_idx('ankle_4')] = 1.0
return qpos
def _sample_state(self):
"""
@brief Returns a random valid initial state.
@details The default implementation only return the neural configuration,
with offsets on the freeflyer to ensure no contact points are
going through the ground and a single one is touching it.
"""
q0 = neutral(self.robot.pinocchio_model)
if self.robot.has_freeflyer:
ground_fun = self.engine.get_options()['world']['groundProfile']
compute_freeflyer_state_from_fixed_body(
self.robot, q0, ground_profile=ground_fun, use_theoretical_model=False)
v0 = np.zeros(self.robot.nv)
x0 = np.concatenate((q0, v0))
return x0
def neutral_state(
robot: jiminy.Model,
split: bool = False
) -> Union[Tuple[np.ndarray, np.ndarray], np.ndarray]:
"""
@brief Return the neutral state of the robot, namely zero joint
positions and unit quaternions regarding the configuration, and
zero velocity vector.
@param robot Robot for which to comput the neutral state.
"""
q0 = neutral(robot.pinocchio_model)
v0 = np.zeros(robot.nv)
if split:
return q0, v0
else:
return np.concatenate((q0, v0))
def _neutral(self):
def joint_position_idx(joint_name):
joint_idx = self.robot.pinocchio_model.getJointId(joint_name)
return self.robot.pinocchio_model.joints[joint_idx].idx_q
qpos = neutral(self.robot.pinocchio_model)
qpos[joint_position_idx('back_bky')] = DEFAULT_SAGITTAL_HIP_ANGLE
qpos[joint_position_idx('l_arm_elx')] = DEFAULT_SAGITTAL_HIP_ANGLE
qpos[joint_position_idx('l_arm_shx')] = -np.pi / 2 + 1e-6
qpos[joint_position_idx('l_arm_shz')] = np.pi / 4 - 1e-6
qpos[joint_position_idx('l_arm_ely')] = np.pi / 4 + np.pi / 2
qpos[joint_position_idx('r_arm_elx')] = -DEFAULT_SAGITTAL_HIP_ANGLE
qpos[joint_position_idx('r_arm_shx')] = np.pi / 2 - 1e-6
qpos[joint_position_idx('r_arm_shz')] = -np.pi / 4 + 1e-6
qpos[joint_position_idx('r_arm_ely')] = np.pi / 4 + np.pi / 2
return qpos
def set_init_config(self):
model = self.model
data = self.data
NQ = model.nq
NV = model.nv
self.q, self.dq, self.ddq, tau = zero(NQ), zero(NV), zero(NV), zero(NV)
self.q = pinocchio.neutral(self.robot.model)
self.q[2] = self.floor_height
# Set initial configuration
angle = np.deg2rad(60.0)
q_dummy = np.zeros(self.num_ctrl_joints)
q_dummy[:] = angle
q_dummy[::2] = -0.5 * angle
self.q[7:] = q_dummy.reshape([self.num_ctrl_joints, 1])
# print(self.q)
self.set_configuration(self.q)
def test_basic(self):
model = self.model
q_ones = np.ones((model.nq))
self.assertFalse(pin.isNormalized(model, q_ones))
self.assertFalse(pin.isNormalized(model, q_ones, 1e-8))
self.assertTrue(pin.isNormalized(model, q_ones, 1e2))
q_rand = np.random.rand((model.nq))
q_rand = pin.normalize(model, q_rand)
self.assertTrue(pin.isNormalized(model, q_rand))
self.assertTrue(pin.isNormalized(model, q_rand, 1e-8))
self.assertTrue(abs(np.linalg.norm(q_rand[3:7]) - 1.) <= 1e-8)
q_next = pin.integrate(model, self.q, np.zeros((model.nv)))
self.assertApprox(q_next, self.q)
v_diff = pin.difference(model, self.q, q_next)
self.assertApprox(v_diff, np.zeros((model.nv)))
q_next = pin.integrate(model, self.q, self.v)
q_int = pin.interpolate(model, self.q, q_next, 0.5)
self.assertApprox(q_int, q_int)
value = pin.squaredDistance(model, self.q, self.q)
self.assertTrue((value <= 1e-8).all())
dist = pin.distance(model, self.q, self.q)
self.assertTrue(dist <= 1e-8)
q_neutral = pin.neutral(model)
self.assertApprox(q_neutral, q_neutral)
q_rand1 = pin.randomConfiguration(model)
q_rand2 = pin.randomConfiguration(model, -np.ones((model.nq)),
np.ones((model.nq)))
self.assertTrue(pin.isSameConfiguration(model, self.q, self.q, 1e-8))
self.assertFalse(pin.isSameConfiguration(model, q_rand1, q_rand2,
1e-8))
def test_std_vector_field(self):
model = self.model
data = self.data
q = pin.neutral(model)
pin.centerOfMass(model, data, q)
com_list = data.com.tolist()
com = data.com[0]
with self.assertRaises(Exception) as context:
com = data.com[len(data.com) + 10]
print("com: ", com)
self.assertTrue('Index out of range' in str(context.exception))
with self.assertRaises(Exception) as context:
com = data.com['1']
print("com: ", com)
self.assertTrue('Invalid index type' in str(context.exception))
def __init__(self):
if self.urdf_filename:
if self.sdf_filename:
raise AttributeError("Please choose between URDF *or* SDF")
df_path = join(self.path, self.urdf_subpath, self.urdf_filename)
builder = RobotWrapper.BuildFromURDF
else:
df_path = join(self.path, self.sdf_subpath, self.sdf_filename)
try:
builder = RobotWrapper.BuildFromSDF
except AttributeError:
raise ImportError("Building SDF models require pinocchio >= 3.0.0")
if self.model_path is None:
self.model_path = getModelPath(df_path, self.verbose)
self.df_path = join(self.model_path, df_path)
self.robot = builder(self.df_path, [join(self.model_path, '../..')],
pin.JointModelFreeFlyer() 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.robot.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose,
self.has_rotor_parameters, self.ref_posture)
if pin.WITH_HPP_FCL and pin.WITH_HPP_FCL_BINDINGS:
# Add all collision pairs
self.robot.collision_model.addAllCollisionPairs()
# Remove collision pairs per SRDF
pin.removeCollisionPairs(self.robot.model, self.robot.collision_model, self.srdf_path, False)
# Recreate collision data since the collision pairs changed
self.robot.collision_data = self.robot.collision_model.createData()
else:
self.srdf_path = None
self.robot.q0 = pin.neutral(self.robot.model)
if self.free_flyer:
self.addFreeFlyerJointLimits()
def __init__(self, q=None):
super(Quadruped12Wrapper, self).__init__()
self.effs = ["FR", "FL", "HR", "HL"] # order is important
self.colors = {"HL": "r", "HR": "y", "FL": "b", "FR": "g"}
self.joints_list = ["HAA", "HFE", "KFE", "ANKLE"]
self.floor_height = 0.
self.robot = Solo12Config.buildRobotWrapper()
self.num_ctrl_joints = 12
# Create data again after setting frames
self.model = self.robot.model
self.data = self.robot.data
q = pinocchio.neutral(self.robot.model)
if not q is None:
self.set_configuration(q)
else:
self.q = None
self.M_com = None
self.mass = sum([i.mass for i in self.model.inertias[1:]])
self.set_init_config()
def _neutral(self):
def set_joint_rotary_position(joint_name, q_full, theta):
joint_idx = self.robot.pinocchio_model.getJointId(joint_name)
joint = self.robot.pinocchio_model.joints[joint_idx]
joint_type = get_joint_type(self.robot.pinocchio_model, joint_idx)
if joint_type == joint_t.ROTARY_UNBOUNDED:
q_joint = np.array([math.cos(theta), math.sin(theta)])
else:
q_joint = theta
q_full[joint.idx_q + np.arange(joint.nq)] = q_joint
qpos = neutral(self.robot.pinocchio_model)
for s in ['left', 'right']:
set_joint_rotary_position(f'hip_flexion_{s}', qpos,
DEFAULT_SAGITTAL_HIP_ANGLE)
set_joint_rotary_position(f'knee_joint_{s}', qpos,
DEFAULT_KNEE_ANGLE)
set_joint_rotary_position(f'ankle_joint_{s}', qpos,
DEFAULT_ANKLE_ANGLE)
set_joint_rotary_position(f'toe_joint_{s}', qpos,
DEFAULT_TOE_ANGLE)
return qpos
# Extract some constant
iPos = robot.motors_position_idx[0]
iVel = robot.motors_velocity_idx[0]
axisCom = 1
# Constants
m = 75
l = 1.0
g = 9.81
omega = np.sqrt(g / l)
# Initial values
q0 = 0.0
dq0 = 0.0
x0 = np.zeros((robot.nq + robot.nv, ))
x0[:robot.nq] = pin.neutral(robot.pinocchio_model_th)
x0[iPos] = q0
x0[iPos + iVel] = dq0
# Compute com dcm references
nTimes = int(tf * 1.0e3) + 1
deltaStabilization = 0.5e3
deltaSlope = 1.0
deltaCom = 0.041
comRef = np.zeros(nTimes)
comRef[int(deltaStabilization):(int(deltaStabilization + deltaSlope * 1.0e3) + 1)] = \
np.linspace(0, deltaCom, int(deltaSlope * 1.0e3) + 1, endpoint=False)
comRef[(int(deltaStabilization + deltaSlope * 1.0e3) + 1):] = deltaCom
zmpRef = comRef
dcomRef = np.zeros(nTimes)
dcomRef[int(deltaStabilization):(int(deltaStabilization + deltaSlope * 1.0e3) +
def initializeRobot(self):
"""
If the robot model is correctly loaded, this method will then
initialize the operational points, set the position to
half-sitting with null velocity/acceleration.
To finish, different tasks are initialized:
- the center of mass task used to keep the robot stability
- one task per operational point to ease robot control
"""
if not hasattr(self, 'dynamic'):
raise RuntimeError("Dynamic robot model must be initialized first")
if not hasattr(self, 'device') or self.device is None:
# raise RuntimeError("A device is already defined.")
self.device = RobotSimu(self.name + '_device')
self.device.resize(self.dynamic.getDimension())
"""
Robot timestep
"""
self.timeStep = self.device.getTimeStep()
# Compute half sitting configuration
import numpy
"""
Half sitting configuration.
"""
self.halfSitting = pinocchio.neutral(self.pinocchioModel)
self.defineHalfSitting(self.halfSitting)
self.halfSitting = numpy.array(
self.halfSitting[:3].tolist() +
[0., 0., 0.] # Replace quaternion by RPY.
+ self.halfSitting[7:].tolist())
assert self.halfSitting.shape[0] == self.dynamic.getDimension()
# Set the device limits.
def get(s):
s.recompute(0)
return s.value
def opposite(v):
return [-x for x in v]
self.dynamic.add_signals()
self.device.setPositionBounds(get(self.dynamic.lowerJl),
get(self.dynamic.upperJl))
self.device.setVelocityBounds(-get(self.dynamic.upperVl),
get(self.dynamic.upperVl))
self.device.setTorqueBounds(-get(self.dynamic.upperTl),
get(self.dynamic.upperTl))
# Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to
# position in freeflyer frame.
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.velocityDerivator.dt.value = self.timeStep
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = numpy.zeros([
self.dimension,
])
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
self.accelerationDerivator.dt.value = self.timeStep
plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = numpy.zeros([
self.dimension,
])
from __future__ import print_function
import numpy as np
from numpy.linalg import norm, solve
import pinocchio
model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(
"/home/gabriele/catkin_ws/src/abel_move/scripts/abel_arms_full.urdf")
data, collision_data, visual_data = pinocchio.createDatas(
model, collision_model, visual_model)
JOINT_ID = 5
oMdes = pinocchio.SE3(np.eye(3), np.array([-0.19, 0.50, -0.38]))
q = pinocchio.neutral(model)
eps = 1e-2
IT_MAX = 1000
DT = 1e-4
damp = 1e-12
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
# Create a 7DOF manipulator robot and moves it along a constant (random) velocity during 10secs.
import numpy as np
from numpy.linalg import pinv, norm
from pinocchio import neutral
from robot_arm import Robot
import time
# Create a 7DOF robot.
robot = Robot()
# Hide the floor.
robot.viewer.viewer.gui.setVisibility('world/floor', 'OFF')
# Move the robot during 10secs at velocity v.
dt = 1e-3
for j in range(robot.model.nv):
v = np.array(robot.model.nv * [0])
v[j] = 1
q = neutral(robot.model)
for i in range(1000):
q += v * dt
robot.display(q)
time.sleep(dt)
for i in range(1000):
q -= v * dt
robot.display(q)
time.sleep(dt)
def get_neutral(self):
return pin.neutral(self.model)
# Start a new MeshCat server and client.
# Note: the server can also be started separately using the "meshcat-server" command in a terminal:
# this enables the server to remain active after the current script ends.
#
# Option open=True pens the visualizer.
# Note: the visualizer can also be opened seperately by visiting the provided URL.
try:
viz.initViewer(open=True)
except ImportError as err:
print(
"Error while initializing the viewer. It seems you should install Python meshcat"
)
print(err)
sys.exit(0)
# Load the robot in the viewer.
viz.loadViewerModel()
# Display a robot configuration.
q0 = pin.neutral(model)
viz.display(q0)
# Display another robot.
viz2 = MeshcatVisualizer(model, collision_model, visual_model)
viz2.initViewer(viz.viewer)
viz2.loadViewerModel(rootNodeName="pinocchio2")
q = q0.copy()
q[1] = 1.0
viz2.display(q)
def zero(self):
q = pinocchio.neutral(self.model)
v = pinocchio.utils.zero(self.nv)
return np.concatenate([q, v])
def zero(self):
""" Return the neutral robot configuration with zero velocity.
"""
q = pinocchio.neutral(self.model)
v = np.zeros(self.model.nv)
return np.concatenate([q.flat, v])
from __future__ import print_function
import numpy as np
import pinocchio
model = pinocchio.buildSampleModelManipulator()
data = model.createData()
JOINT_ID = 6
xdes = np.matrix([ 0.5,-0.5,0.5]).T
q = pinocchio.neutral(model)
eps = 1e-4
IT_MAX = 1000
DT = 1e-1
i=0
while True:
pinocchio.forwardKinematics(model,data,q)
x = data.oMi[JOINT_ID].translation
R = data.oMi[JOINT_ID].rotation
err = R.T*(x-xdes)
if np.linalg.norm(err) < eps:
print("Convergence achieved!")
break
if i >= IT_MAX:
print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")
break
J = pinocchio.jointJacobian(model,data,q,JOINT_ID)[:3,:]
v = - np.linalg.pinv(J)*err
q = pinocchio.integrate(model,q,v*DT)
def neutral_configuration(self):
q = pin.neutral(self._model)
qw = ConfigurationWrapper(self, q)
return qw
