def test_multi_load(self):
hint_list = [self.model_dir, "wrong/hint"]
expected_collision_path = os.path.join(
self.model_dir,
'romeo_description/meshes/V1/collision/LHipPitch.dae')
expected_visual_path = os.path.join(
self.model_dir, 'romeo_description/meshes/V1/visual/LHipPitch.dae')
model = pin.buildModelFromUrdf(self.model_path,
pin.JointModelFreeFlyer())
collision_model = pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.COLLISION,
hint_list)
col = collision_model.geometryObjects[1]
self.assertEqual(col.meshPath, expected_collision_path)
visual_model = pin.buildGeomFromUrdf(model, self.model_path,
pin.GeometryType.VISUAL,
hint_list)
vis = visual_model.geometryObjects[1]
self.assertEqual(vis.meshPath, expected_visual_path)
model_2, collision_model_2, visual_model_2 = pin.buildModelsFromUrdf(
self.model_path, hint_list, pin.JointModelFreeFlyer())
self.assertEqual(model, model_2)
col_2 = collision_model_2.geometryObjects[1]
self.assertEqual(col_2.meshPath, expected_collision_path)
vis_2 = visual_model_2.geometryObjects[1]
self.assertEqual(vis_2.meshPath, expected_visual_path)
model_c, collision_model_c = pin.buildModelsFromUrdf(
self.model_path,
hint_list,
pin.JointModelFreeFlyer(),
geometry_types=pin.GeometryType.COLLISION)
self.assertEqual(model, model_c)
col_c = collision_model_c.geometryObjects[1]
self.assertEqual(col_c.meshPath, expected_collision_path)
model_v, visual_model_v = pin.buildModelsFromUrdf(
self.model_path,
hint_list,
pin.JointModelFreeFlyer(),
geometry_types=pin.GeometryType.VISUAL)
self.assertEqual(model, model_v)
vis_v = visual_model_v.geometryObjects[1]
self.assertEqual(vis_v.meshPath, expected_visual_path)
def _config_robot(self, urdf_file, package_dir):
if self._b_fixed_base:
# Fixed based robot
self._model, self._collision_model, self._visual_model = pin.buildModelsFromUrdf(
urdf_file, package_dir)
self._n_floating = 0
else:
# Floating based robot
self._model, self._collision_model, self._visual_model = pin.buildModelsFromUrdf(
urdf_file, package_dir, pin.JointModelFreeFlyer())
self._n_floating = 6
self._data, self._collision_data, self._visual_data = pin.createDatas(
self._model, self._collision_model, self._visual_model)
self._n_q = self._model.nq
self._n_q_dot = self._model.nv
self._n_a = self._n_q_dot - self._n_floating
passing_idx = 0
for j_id, j_name in enumerate(self._model.names):
if j_name == 'root_joint' or j_name == 'universe':
passing_idx += 1
else:
self._joint_id[j_name] = j_id - passing_idx
for f_id, frame in enumerate(self._model.frames):
if frame.name == 'root_joint' or frame.name == 'universe':
pass
else:
if f_id % 2 == 0:
# Link
link_id = int(f_id / 2 - 1)
self._link_id[frame.name] = link_id
else:
# Joint
pass
assert len(self._joint_id) == self._n_a
self._total_mass = sum(
[inertia.mass for inertia in self._model.inertias])
self._joint_pos_limit = np.stack(
[self._model.lowerPositionLimit, self._model.upperPositionLimit],
axis=1)[self._n_floating:self._n_floating + self._n_a, :]
self._joint_vel_limit = np.stack(
[-self._model.velocityLimit, self._model.velocityLimit],
axis=1)[self._n_floating:self._n_floating + self._n_a, :]
self._joint_trq_limit = np.stack(
[-self._model.effortLimit, self._model.effortLimit],
axis=1)[self._n_floating:self._n_floating + self._n_a, :]
def appendUrdfModel(self, urdfFilename, frame_name, fMm, prefix=None):
if not isinstance(fMm, pinocchio.SE3):
fMm = pinocchio.XYZQUATToSE3(fMm)
id_parent_frame = self.model.getFrameId(frame_name)
model, gmodel = pinocchio.buildModelsFromUrdf(
hpp.rostools.retrieve_resource(urdfFilename),
geometry_types=pinocchio.GeometryType.COLLISION)
if prefix is not None:
for i in range(1, len(model.names)):
model.names[i] = prefix + model.names[i]
for f in model.frames:
f.name = prefix + f.name
for go in gmodel.geometryObjects:
go.name = prefix + go.name
igeom = self.gmodel.ngeoms
nmodel, ngmodel = pinocchio.appendModel(self.model, model, self.gmodel,
gmodel, id_parent_frame, fMm)
self.gid_field_of_view = ngmodel.getGeometryId("field_of_view")
for go in gmodel.geometryObjects:
ngmodel.addCollisionPair(
pinocchio.CollisionPair(self.gid_field_of_view,
ngmodel.getGeometryId(go.name)))
self.model, self.gmodel = nmodel, ngmodel
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
def __init__(
self,
urdfString=None,
urdfFilename=None,
fov=np.radians((60., 90.)),
# fov = np.radians((49.5, 60)),
geoms=["arm_3_link_0"],
):
if isinstance(fov, str):
self.fov = fov
fov_fcl = hppfcl.MeshLoader().load(
hpp.rostools.retrieve_resource(fov))
else:
# build FOV tetahedron
dx, dy = np.sin(fov)
pts = hppfcl.StdVec_Vec3f()
self.fov = [
(0, 0, 0),
(dx, dy, 1),
(-dx, dy, 1),
(-dx, -dy, 1),
(dx, -dy, 1),
]
pts.extend([np.array(e) for e in self.fov])
self.fov.append(self.fov[1])
fov_fcl = hppfcl.BVHModelOBBRSS()
fov_fcl.beginModel(4, 5)
fov_fcl.addSubModel(pts, _tetahedron_tris)
fov_fcl.endModel()
self.cos_angle_thr = np.cos(np.radians(70))
if urdfFilename is None:
assert urdfString is not None
# Pinocchio does not allow to build a GeometryModel from a XML string.
urdfFilename = '/tmp/tmp.urdf'
with open(urdfFilename, 'w') as f:
f.write(urdfString)
self.model, self.gmodel = pinocchio.buildModelsFromUrdf(
hpp.rostools.retrieve_resource(urdfFilename),
root_joint=pinocchio.JointModelPlanar(),
geometry_types=pinocchio.GeometryType.COLLISION)
id_parent_frame = self.model.getFrameId("xtion_rgb_optical_frame")
parent_frame = self.model.frames[id_parent_frame]
go = pinocchio.GeometryObject("field_of_view", id_parent_frame,
parent_frame.parent, fov_fcl,
parent_frame.placement)
self.gid_field_of_view = self.gmodel.addGeometryObject(go, self.model)
for n in geoms:
assert self.gmodel.existGeometryName(n)
self.gmodel.addCollisionPair(
pinocchio.CollisionPair(self.gmodel.getGeometryId(n),
self.gid_field_of_view))
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
def __init__(self, urdf_path, meshes_path):
self.model, self.collision_model, self.visual_model = pin.buildModelsFromUrdf(
urdf_path, meshes_path)
self.viz = MeshcatVisualizer(self.model, self.collision_model,
self.visual_model)
try:
self.viz.initViewer(open=False)
except ImportError as err:
print(err)
exit(0)
self.viz.loadViewerModel()
def inverse_kinematics(self, x, y, z):
"""Calculate the joint values for a desired cartesian position"""
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([x, y, x]))
q = np.array(abel.sort_joints()[5:])
eps = 1e-1
IT_MAX = 1000
DT = 1e-4
damp = 1e-4
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)
v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
q = pinocchio.integrate(model, q, v * DT)
if not i % 10:
print('%d: error = %s' % (i, err.T))
i += 1
if success:
print("Convergence achieved!")
else:
print(
"\nWarning: the iterative algorithm has not reached convergence to the desired precision"
)
print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % err.T)
point = JointTrajectoryPoint()
point.positions = [
0, 0, 0, 0, 0, q[0], q[1], q[2], q[3], q[4], q[5], q[6], q[7],
q[8], q[9]
]
abel.move_all_joints(point)
self.direct_kinematics()
def direct_kinematics(self):
"""
Compute the direct kinematics for left and right hand (end-effector) by using the URDF model
"""
# Create data required by the algorithms
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)
q = np.array(abel.sort_joints()[5:])
pinocchio.forwardKinematics(model, data, q)
for name, oMi in zip(self.model.names, data.oMi):
print(("{:<24} : {: .2f} {: .2f} {: .2f}".format(
name, *oMi.translation.T.flat)))
import pinocchio as pin
import numpy as np
from os.path import *
# Goal: Build a reduced model from an existing URDF model by fixing the desired joints at a specified position.
# Load UR robot arm
# This path refers to Pinocchio source code but you can define your own directory here.
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
model_path = pinocchio_model_dir + '/others/robots'
mesh_dir = model_path
# You should change here to set up your own URDF file
urdf_filename = model_path + '/ur_description/urdf/ur5_robot.urdf'
model, collision_model, visual_model = pin.buildModelsFromUrdf(
urdf_filename, mesh_dir)
# Check dimensions of the original model
print('standard model: dim=' + str(len(model.joints)))
for jn in model.joints:
print(jn)
# Create a list of joints to lock
jointsToLock = ['wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
# Get the ID of all existing joints
jointsToLockIDs = []
for jn in jointsToLock:
if model.existJointName(jn):
jointsToLockIDs.append(model.getJointId(jn))
else:
def __init__(self, controller=None):
"""
Description:
1. Initiates, Builds & Starts Robot Model components.
2. Initializes 'NYUFingerSimulator()' as Visualizer.
Args:
1. connect_hardware = True (default).
Connects to the robot hardware if enabled
else, runs as simulation.
Keyword Args:
1. controller = 'None': Does not initialize a joystick.
'keyboard': Enables joystick mode on the keyboard.
"""
# Load the .urdf files
self.package_dir = os.path.abspath(os.getcwd())
self.urdf_path = self.package_dir + '/model/fingeredu.urdf'
# Create Models from the .urdf files
self.model, self.collision_model, self.visual_model = \
pin.buildModelsFromUrdf(self.urdf_path, self.package_dir)
self.collision_model.addAllCollisionPairs()
# Create datas from models
self.model_data = self.model.createData()
self.collision_model.addAllCollisionPairs()
self.collision_data = pin.GeometryData(self.collision_model)
# Display the models in viewer
self.simulator = NYUFingerSimulator(self.package_dir, self.urdf_path)
# Get the FrameID of the EOAT
self.EOAT_ID = self.model.getFrameId('finger_tip_link')
# Get the FrameID of the Base
self.Base_ID = self.model.getFrameId('base_link')
# Read Initial Joint Values
init_q, init_v = self.simulator.get_state()
# Initial Position
memory.pose = self.ForwardKinematics(init_q, init_v)[0]
# PD Controller
self.control_freq = 0.001
self.PD = SimPD(self.control_freq, self.simulator, self.model,
self.model_data, self.ForwardKinematics, self.Jacobian,
self.TimeDerivativeJacobian, 9500, 35, 1.5, 18000, 100,
50)
# Direct Joint Limits
J1_limits = np.pi / 2
J2_limits = np.pi
J3_limits = np.pi
self.JointLimits = np.array([J1_limits, J2_limits, J3_limits])
# Init. Motion Planner
self.Motion = MotionPlanner(self.model, self.model_data, self.PD,
self.simulator, self.ForwardKinematics,
self.Jacobian, self.JointLimits)
# Feature Configuration Spaces
self.StablePose = init_q
self.parking_pose = np.array([0, -np.pi / 4, np.pi / 4])
# Use Keyboard as a controller
if controller == 'keyboard':
TeleKeyboard = NumStick(self.Motion.LinearPlanner)
TeleKeyboard.run()
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
def __init__(self, conf, viewer=True):
self.conf = conf
vector = se3.StdVec_StdString()
vector.extend(item for item in conf.path)
self.robot = tsid.RobotWrapper(conf.urdf, vector, se3.JointModelFreeFlyer(), False)
robot = self.robot
# print "The path of urdf is ", conf.urdf
# self.model = robot.model()
filename = str(Path(os.path.abspath(__file__)).parents[1])
print "The filename is ", filename
path = filename + '/models/romeo'
mesh_dir = path
model, collision_model, visual_model = se3.buildModelsFromUrdf(conf.urdf, mesh_dir,
se3.JointModelFreeFlyer())
self.model = model
pin.loadReferenceConfigurations(self.model, conf.srdf, False)
self.q0 = q = self.model.referenceConfigurations["half_sitting"]
v = np.matrix(np.zeros(robot.nv)).T
assert self.model.existFrame(conf.rf_frame_name)
assert self.model.existFrame(conf.lf_frame_name)
formulation = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
contact_Point = np.matrix(np.ones((3,4)) * conf.lz)
contact_Point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp]
contact_Point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp]
contactRF =tsid.Contact6d("contact_rfoot", robot, conf.rf_frame_name, contact_Point,
conf.contactNormal, conf.mu, conf.fMin, conf.fMax)
contactRF.setKp(conf.kp_contact * matlib.ones(6).T)
contactRF.setKd(2.0 * np.sqrt(conf.kp_contact) * matlib.ones(6).T)
self.RF = self.model.getJointId(conf.rf_frame_name)
H_rf_ref = robot.position(data, self.RF)
# modify initial robot configuration so that foot is on the ground (z=0)
q[2] -= H_rf_ref.translation[2,0] - conf.lz
formulation.computeProblemData(0.0, q, v)
data = self.model.createData()
# H_rf_ref = se3.forwardDynamics(self.model, data, q, self.RF)
# data = formulation.data()
H_rf_ref = robot.position(data, self.RF)
contactRF.setReference(H_rf_ref)
formulation.addRigidContact(contactRF, conf.w_forceRef)
contactLF =tsid.Contact6d("contact_lfoot", robot, conf.lf_frame_name, contact_Point,
conf.contactNormal, conf.mu, conf.fMin, conf.fMax)
contactLF.setKp(conf.kp_contact * matlib.ones(6).T)
contactLF.setKd(2.0 * np.sqrt(conf.kp_contact) * matlib.ones(6).T)
self.LF = self.model.getJointId(conf.lf_frame_name)
H_lf_ref = robot.position(data, self.LF)
contactLF.setReference(H_lf_ref)
formulation.addRigidContact(contactLF, conf.w_forceRef)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(conf.kp_com * matlib.ones(3).T)
comTask.setKd(2.0 * np.sqrt(conf.kp_com) * matlib.ones(3).T)
formulation.addMotionTask(comTask, conf.w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(conf.kp_posture * matlib.ones(robot.nv-6).T)
postureTask.setKd(2.0 * np.sqrt(conf.kp_posture) * matlib.ones(robot.nv-6).T)
formulation.addMotionTask(postureTask, conf.w_posture, 1, 0.0)
self.leftFootTask = tsid.TaskSE3Equality("task-left-foot", self.robot, self.conf.lf_frame_name)
self.leftFootTask.setKp(self.conf.kp_foot * np.matrix(np.ones(6)).T)
self.leftFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.matrix(np.ones(6)).T)
self.trajLF = tsid.TrajectorySE3Constant("traj-left-foot", H_lf_ref)
formulation.addMotionTask(self.leftFootTask, self.conf.w_foot, 1, 0.0)
self.rightFootTask = tsid.TaskSE3Equality("task-right-foot", self.robot, self.conf.rf_frame_name)
self.rightFootTask.setKp(self.conf.kp_foot * np.matrix(np.ones(6)).T)
self.rightFootTask.setKd(2.0 * np.sqrt(self.conf.kp_foot) * np.matrix(np.ones(6)).T)
self.trajRF = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref)
formulation.addMotionTask(self.rightFootTask, self.conf.w_foot, 1, 0.0)
self.tau_max = conf.tau_max_scaling*self.model.effortLimit[-robot.na:]
self.tau_min = -self.tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
if(conf.w_torque_bounds>0.0):
formulation.addActuationTask(actuationBoundsTask, conf.w_torque_bounds, 0, 0.0)
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, conf.dt)
self.v_max = conf.v_max_scaling * self.model.velocityLimit[-robot.na:]
self.v_min = -self.v_max
jointBoundsTask.setVelocityBounds(self.v_min, self.v_max)
if(conf.w_joint_bounds>0.0):
formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0, 0.0)
com_ref = robot.com(data)
self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
self.sample_com = self.trajCom.computeNext()
q_ref = q[7:]
self.trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
postureTask.setReference(self.trajPosture.computeNext())
self.sampleLF = self.trajLF.computeNext()
self.sample_LF_pos = self.sampleLF.pos()
self.sample_LF_vel = self.sampleLF.vel()
self.sample_LF_acc = self.sampleLF.acc()
self.sampleRF = self.trajRF.computeNext()
self.sample_RF_pos = self.sampleRF.pos()
self.sample_RF_vel = self.sampleRF.vel()
self.sample_RF_acc = self.sampleRF.acc()
self.solver = tsid.SolverHQuadProgFast("qp solver")
self.solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.comTask = comTask
self.postureTask = postureTask
self.contactRF = contactRF
self.contactLF = contactLF
self.actuationBoundsTask = actuationBoundsTask
self.jointBoundsTask = jointBoundsTask
self.formulation = formulation
self.q = q
self.v = v
self.contact_LF_active = True
self.contact_RF_active = True
# for gepetto viewer
if(viewer):
self.robot_display = se3.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ], se3.JointModelFreeFlyer())
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
gepetto.corbaserver.Client()
self.robot_display.initDisplay(loadModel=True)
self.robot_display.displayCollisions(False)
self.robot_display.displayVisuals(True)
self.robot_display.display(q)
self.gui = self.robot_display.viewer.gui
self.gui.setCameraTransform(0, conf.CAMERA_TRANSFORM)
self.gui.addFloor('world/floor')
self.gui.setLightingMode('world/floor', 'OFF');
# Create Robot, Ground
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
robot = p.loadURDF(cwd + "/robot_model/atlas/atlas.urdf",
SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT,
SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT)
p.loadURDF(cwd + "/robot_model/ground/plane.urdf", [0, 0, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
nq, nv, na, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom = pybullet_util.get_robot_config(
robot, SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT,
SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT, True)
# Create Robot for Meshcat Visualization
model, collision_model, visual_model = pin.buildModelsFromUrdf(
cwd + "/robot_model/atlas/atlas.urdf", cwd + "/robot_model/atlas",
pin.JointModelFreeFlyer())
viz = MeshcatVisualizer(model, collision_model, visual_model)
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)
viz.loadViewerModel()
vis_q = pin.neutral(model)
# Initial Config
set_initial_config(robot, joint_id)
addr = config["ip_addr"]
context = zmq.Context()
pnc_socket = context.socket(zmq.SUB)
pnc_socket.connect(addr)
pnc_socket.setsockopt_string(zmq.SUBSCRIBE, "")
# pj_context = zmq.Context()
# pj_socket = pj_context.socket(zmq.PUB)
# pj_socket.bind("tcp://*:9872")
data_saver = DataSaver()
if args.b_visualize:
model, collision_model, visual_model = pin.buildModelsFromUrdf(
"robot_model/draco/draco.urdf", "robot_model/draco",
pin.JointModelFreeFlyer())
viz = MeshcatVisualizer(model, collision_model, visual_model)
try:
viz.initViewer(open=True)
except ImportError as err:
print(
"Error while initializing the viewer. It seems you should install python meshcat"
)
print(err)
exit()
viz.loadViewerModel()
vis_q = pin.neutral(model)
msg = pnc_msg()
if ManipulatorConfig.VIDEO_RECORD:
if not os.path.exists('video'):
os.makedirs('video')
for f in os.listdir('video'):
os.remove('video/' + f)
p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "video/atlas.mp4")
# Create Robot, Ground
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
robot = p.loadURDF(cwd +
"/robot_model/manipulator/three_link_manipulator.urdf",
useFixedBase=True)
# Create Robot for Meshcat Visualization
model, collision_model, visual_model = pin.buildModelsFromUrdf(
cwd + "/robot_model/manipulator/three_link_manipulator.urdf",
cwd + "/robot_model/manipulator")
viz = MeshcatVisualizer(model, collision_model, visual_model)
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)
viz.loadViewerModel()
vis_q = pin.neutral(model)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
nq, nv, na, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom = pybullet_util.get_robot_config(
p.changeConstraint(c, gearRatio=-1, maxForce=500, erp=10)
c = p.createConstraint(robot,
link_id['r_knee_fe_lp'],
robot,
link_id['r_knee_fe_ld'],
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=500, erp=10)
if SimConfig.B_USE_MESHCAT:
# Create Robot for Meshcat Visualization
model, collision_model, visual_model = pin.buildModelsFromUrdf(
cwd + "/robot_model/draco3/draco3.urdf",
cwd + "/robot_model/draco3", pin.JointModelFreeFlyer())
viz = MeshcatVisualizer(model, collision_model, visual_model)
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)
viz.loadViewerModel()
vis_q = pin.neutral(model)
# Initial Config
set_initial_config(robot, joint_id)
import pinocchio
pinocchio.switchToNumpyMatrix()
from sys import argv
from os.path import dirname, join, abspath
# This path refers to Pinocchio source code but you can define your own directory here.
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
model_path = join(pinocchio_model_dir,
"others/robots") if len(argv) < 2 else argv[1]
mesh_dir = model_path
urdf_model_path = join(model_path, "ur_description/urdf/ur5_robot.urdf")
# Load the urdf model
model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(
urdf_model_path, mesh_dir)
print('model name: ' + model.name)
# Create data required by the algorithms
data, collision_data, visual_data = pinocchio.createDatas(
model, collision_model, visual_model)
# Sample a random configuration
q = pinocchio.randomConfiguration(model)
print('q: %s' % q.T)
# Perform the forward kinematics over the kinematic tree
pinocchio.forwardKinematics(model, data, q)
# Update Geometry models
pinocchio.updateGeometryPlacements(model, data, collision_model,
import math
import copy
from collections import OrderedDict
import pinocchio as pin
from pinocchio.robot_wrapper import RobotWrapper
import numpy as np
from pinocchio.visualize import MeshcatVisualizer
# urdf_filename = cwd + "/robot_model/atlas/atlas.urdf"
# package_dir = cwd + "/robot_model/atlas/"
urdf_filename = cwd + "/robot_model/valkyrie/valkyrie.urdf"
package_dir = cwd + "/robot_model/valkyrie/"
model, collision_model, visual_model = pin.buildModelsFromUrdf(
urdf_filename, package_dir, pin.JointModelFreeFlyer())
viz = MeshcatVisualizer(model, collision_model, visual_model)
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)
viz.loadViewerModel()
q0 = pin.neutral(model)
import sys
import os
from os.path import dirname, join, abspath
from pinocchio.visualize import MeshcatVisualizer
# Load the URDF model.
# Conversion with str seems to be necessary when executing this file with ipython
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
model_path = join(pinocchio_model_dir, "example-robot-data/robots")
mesh_dir = pinocchio_model_dir
urdf_filename = "talos_reduced.urdf"
urdf_model_path = join(join(model_path, "talos_data/robots"), urdf_filename)
model, collision_model, visual_model = pin.buildModelsFromUrdf(
urdf_model_path, mesh_dir, pin.JointModelFreeFlyer())
viz = MeshcatVisualizer(model, collision_model, visual_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"
)
addr = config["ip_addr"]
context = zmq.Context()
pnc_socket = context.socket(zmq.SUB)
pnc_socket.connect(addr)
pnc_socket.setsockopt_string(zmq.SUBSCRIBE, "")
# pj_context = zmq.Context()
# pj_socket = pj_context.socket(zmq.PUB)
# pj_socket.bind("tcp://*:9872")
data_saver = DataSaver()
if args.b_visualize:
model, collision_model, visual_model = pin.buildModelsFromUrdf(
"robot_model/draco/draco.urdf", "robot_model/draco",
pin.JointModelFreeFlyer())
viz = MeshcatVisualizer(model, collision_model, visual_model)
try:
viz.initViewer(open=True)
except ImportError as err:
print(
"Error while initializing the viewer. It seems you should install python meshcat"
)
print(err)
exit()
viz.loadViewerModel()
vis_q = pin.neutral(model)
icp_model, icp_collision_model, icp_visual_model = pin.buildModelsFromUrdf(
"robot_model/ground/sphere.urdf", "robot_model/ground",