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",