def track_com(self): """Instead of tracking the biased base position and velocity, track the biased com and vcom position. """ # Init a dg pinocchio robot here for com and vcom computation. self.robot_pin = Solo12Config.buildRobotWrapper() self.robot_dg = dp.DynamicPinocchio(self.EntityName + '_pinocchio') self.robot_dg.setModel(self.robot_pin.model) self.robot_dg.setData(self.robot_pin.data) self.robot_position = stack_two_vectors( self.get_biased_base_position(), self.robot.device.joint_positions, 6, 12) self.robot_velocity = stack_two_vectors( self.get_biased_base_velocity(), self.robot.device.joint_velocities, 6, 12) dg.plug(self.robot_position, self.robot_dg.position) dg.plug(self.robot_velocity, self.robot_dg.velocity) self.robot_dg.acceleration.value = 18 * [ 0., ] # These are used in the reactive_stepper. self.robot_com = vectorIdentity(self.robot_dg.com, 3, self.EntityName + '_pin_com') self.robot_vcom = multiply_mat_vec(self.robot_dg.Jcom, self.robot_velocity, self.EntityName + '_pin_vcom') dg.plug(self.robot_com, self.com_imp_ctrl.biased_pos) dg.plug(self.robot_vcom, self.com_imp_ctrl.biased_vel)
def __init__(self, physicsClient=None): if physicsClient is None: self.physicsClient = self.initPhysicsClient() # Load the plain. plain_urdf = (get_package_share_directory("robot_properties_solo") + "/urdf/plane_with_restitution.urdf") self.planeId = pybullet.loadURDF(plain_urdf) # Load the robot robotStartPos = [0.0, 0, 0.40] robotStartOrientation = pybullet.getQuaternionFromEuler([0, 0, 0]) self.urdf_path = Solo12Config.urdf_path self.robotId = pybullet.loadURDF( self.urdf_path, robotStartPos, robotStartOrientation, flags=pybullet.URDF_USE_INERTIA_FROM_FILE, useFixedBase=False, ) pybullet.getBasePositionAndOrientation(self.robotId) # Create the robot wrapper in pinocchio. self.pin_robot = Solo12Config.buildRobotWrapper() # Query all the joints. num_joints = pybullet.getNumJoints(self.robotId) for ji in range(num_joints): pybullet.changeDynamics( self.robotId, ji, linearDamping=0.04, angularDamping=0.04, restitution=0.0, lateralFriction=0.5, ) self.base_link_name = "base_link" controlled_joints = [] for leg in ["FL", "FR", "HL", "HR"]: controlled_joints += [leg + "_HAA", leg + "_HFE", leg + "_KFE"] self.joint_names = controlled_joints # Creates the wrapper by calling the super.__init__. super(Solo12Robot, self).__init__( self.robotId, self.pin_robot, controlled_joints, ["FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"], )
def __init__(self, physicsClient=None): if physicsClient is None: self.physicsClient = self.initPhysicsClient() # Load the plain. plain_urdf = (rospkg.RosPack().get_path("robot_properties_solo") + "/urdf/plane_with_restitution.urdf") self.planeId = p.loadURDF(plain_urdf) # Load the robot robotStartPos = [0., 0, 0.40] robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) self.urdf_path = Solo12Config.urdf_path self.robotId = p.loadURDF(self.urdf_path, robotStartPos, robotStartOrientation, flags=p.URDF_USE_INERTIA_FROM_FILE, useFixedBase=False) p.getBasePositionAndOrientation(self.robotId) # Create the robot wrapper in pinocchio. package_dirs = [ os.path.dirname(os.path.dirname(self.urdf_path)) + '/urdf' ] self.pin_robot = Solo12Config.buildRobotWrapper() # Query all the joints. num_joints = p.getNumJoints(self.robotId) for ji in range(num_joints): p.changeDynamics(self.robotId, ji, linearDamping=.04, angularDamping=0.04, restitution=0.0, lateralFriction=0.5) self.base_link_name = "base_link" controlled_joints = [] for leg in ['FL', 'FR', 'HL', 'HR']: controlled_joints += [leg + '_HAA', leg + '_HFE', leg + '_KFE'] self.joint_names = controlled_joints # Creates the wrapper by calling the super.__init__. super(Quadruped12Robot, self).__init__(self.robotId, self.pin_robot, controlled_joints, ['FL_ANKLE', 'FR_ANKLE', 'HL_ANKLE', 'HR_ANKLE'])
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 __init__(self, leg_name, joint_indices_range): """ Args: leg_name: (str) Name of the leg, like "FL" joint_indices_range: (array) Indices on the joint array of this leg. """ self.leg_name = leg_name self.joint_indices_range = joint_indices_range self.robot_pin = Solo12Config.buildRobotWrapper() self.robot_dg = dp.DynamicPinocchio(self.leg_name) self.robot_dg.setModel(self.robot_pin.model) self.robot_dg.setData(self.robot_pin.data) self.robot_dg.createJacobianEndEffWorld('jac_cnt_' + self.leg_name, self.leg_name + '_ANKLE') self.robot_dg.createPosition('pos_hip_' + self.leg_name, self.leg_name + '_HFE') self.robot_dg.createPosition('pos_foot_' + self.leg_name, self.leg_name + '_ANKLE') self.robot_dg.acceleration.value = 18 * (0.0, ) self.joint_positions_sin = self.robot_dg.position self.joint_velocities_sin = self.robot_dg.velocity
""" Basic loading and visualization for the Solo robot using gepetto viewer. """ import numpy as np import pinocchio as se3 import time import os from robot_properties_solo.config import Solo12Config # Load the robot urdf. robot = Solo12Config.buildRobotWrapper() # Setup the display (connection to gepetto viewer) and load the robot model. robot.initDisplay(loadModel=True) # Create a first initial position for the robot. Both legs are bent inwards. q = np.matrix(Solo12Config.initial_configuration).T # Turn the legs outside q[[10, 16]] = -0.5 # Right side of quadruped q[[ 7, 13]] = 0.5 # Left side of quadruped # Display the configuration in the viewer. robot.display(q) # Example of moving the robot forward and updating the display every time. for i in range(10): q[0] += 0.05 robot.display(q) time.sleep(0.2)
def __init__( self, pos=None, orn=None, useFixedBase=False, init_sliders_pose=4 * [ 0, ], ): # Load the robot if pos is None: pos = [0.0, 0, 0.40] if orn is None: orn = pybullet.getQuaternionFromEuler([0, 0, 0]) pybullet.setAdditionalSearchPath(Solo12Config.meshes_path) self.urdf_path = Solo12Config.urdf_path self.robotId = pybullet.loadURDF( self.urdf_path, pos, orn, flags=pybullet.URDF_USE_INERTIA_FROM_FILE, useFixedBase=useFixedBase, ) pybullet.getBasePositionAndOrientation(self.robotId) # Create the robot wrapper in pinocchio. self.pin_robot = Solo12Config.buildRobotWrapper() # Query all the joints. num_joints = pybullet.getNumJoints(self.robotId) for ji in range(num_joints): pybullet.changeDynamics( self.robotId, ji, linearDamping=0.04, angularDamping=0.04, restitution=0.0, lateralFriction=0.5, ) self.slider_a = pybullet.addUserDebugParameter("a", 0, 1, init_sliders_pose[0]) self.slider_b = pybullet.addUserDebugParameter("b", 0, 1, init_sliders_pose[1]) self.slider_c = pybullet.addUserDebugParameter("c", 0, 1, init_sliders_pose[2]) self.slider_d = pybullet.addUserDebugParameter("d", 0, 1, init_sliders_pose[3]) self.base_link_name = "base_link" self.end_eff_ids = [] self.end_effector_names = [] controlled_joints = [] for leg in ["FL", "FR", "HL", "HR"]: controlled_joints += [leg + "_HAA", leg + "_HFE", leg + "_KFE"] self.end_eff_ids.append( self.pin_robot.model.getFrameId(leg + "_FOOT")) self.end_effector_names.append(leg + "_FOOT") self.joint_names = controlled_joints self.nb_ee = len(self.end_effector_names) self.hl_index = self.pin_robot.model.getFrameId("HL_ANKLE") self.hr_index = self.pin_robot.model.getFrameId("HR_ANKLE") self.fl_index = self.pin_robot.model.getFrameId("FL_ANKLE") self.fr_index = self.pin_robot.model.getFrameId("FR_ANKLE") # Creates the wrapper by calling the super.__init__. super(Solo12Robot, self).__init__( self.robotId, self.pin_robot, controlled_joints, ["FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"], )