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, use_fixed_base=False, record_video=False,
init_sliders_pose=4*[0.5], hide_gui=False):
super(Solo12BulletRobot, self).__init__(Solo12Config(), use_fixed_base,
record_video, init_sliders_pose)
if hide_gui:
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
self.q0[0] = 0.2
self.q0[1] = 0.0
self.q0[2] = 0.25
self.q0[6] = 1.
self.q0[7] = 0.0
self.q0[8] = 0.8
self.q0[9] = -1.6
self.q0[10] = 0.0
self.q0[11] = 0.8
self.q0[12] = -1.6
self.q0[13] = 0.0
self.q0[14] = -0.8
self.q0[15] = 1.6
self.q0[16] = 0.0
self.q0[17] = -0.8
self.q0[18] = 1.6
# Sync the current robot state to the graph input signals.
self.sim2signal_()
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"],
)