예제 #1
0
    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)
예제 #2
0
    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, :]
예제 #3
0
    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)
예제 #4
0
    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)
예제 #5
0
 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()
예제 #6
0
    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()
예제 #7
0
    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()
예제 #10
0
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
예제 #11
0
    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');
예제 #12
0
    # 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)
예제 #13
0
    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(
예제 #15
0
    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)
예제 #16
0
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,
예제 #17
0
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)
예제 #18
0
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"
    )
예제 #19
0
    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",