def __init__(
self,
manipulator_definition: Union[SMManipulatorDefinition, Dict, str],
):
# assert that all arguments are of the right type.
assert isinstance(
manipulator_definition, (SMManipulatorDefinition, Dict, str)
), f"the manipulator_definition has to be an SMManipulatorDefinition"
if isinstance(manipulator_definition, Dict):
SMManipulatorDefinition.assert_required_fields(
manipulator_definition)
manipulator_definition = SMManipulatorDefinition(
**manipulator_definition)
elif isinstance(manipulator_definition, str):
manipulator_definition.from_file(manipulator_definition)
self.manipulator_definition = manipulator_definition
# create the urdf
self.manipulator_definition.urdf_filename = create_manipulator_urdf(
self.manipulator_definition)
# turn of velocity_control
self.actuators = list(
range(self.manipulator_definition.n_act)
) # actuator_names #actuator_names are act0,act1,... with numbering starting from base. xx fix this comment, i think actuator names are 0,1,...
self.actuator_joints = (
{}
) # a dict that returns all the jointIds belonging to each of the actuators
self.actuatorAxis_joints = (
{}
) # a dict that returns all the joints belonging to a specific axis of an actuator
self.bodyUniqueId = None
self.baseConstraintUniqueId = None
self.flexible_joint_indices = (
[]) # list that contains the indices of all flexible joints
self.joint_controller_limit_forces = (
[]
) # list that contains the joint controller limit force for each flexible joints; same order of joints as self.flexible_joint_indices
# todo consider deleting the two lines below if not needed
# self.main_link_Ids = [] # contains the ids of the main segment links (not the helper links used in the capsule and stadium shapes)
# self.arc_length_by_link_Ids = [] # arc_length_by_link_Ids[i] contains the arc length associated with link main_link_Ids[i] (measured to the center of mass of the link)
self.linkId_to_arcLength = (
[]
) # linkId_to_arcLength[i] contains the summed (cumsum) arc_length from base to the center of link i
# this is for linear passive torque only! by default, everything is initialized as linear springs.
# self.passive_torque_fn = compute_linSpring_joint_torque # xx todo: expand and refactor do make it easy to use other fns. maybe this should be a new class?
# todo: give the option to pass in the torque function?
self.instantiated = False
self.physics_client = None # if the manipulator is not loaded to pybullet, the physics client is None.
def load_exp_to_pybullet(self):
manipulators = []
for manipulator_detail in self.manipulator_details:
(definition_path, startPos, startOr) = manipulator_detail
definition = SMManipulatorDefinition.from_file(definition_path)
manipulator = SMContinuumManipulator(definition)
manipulator.load_to_pybullet(
baseStartPos=startPos,
baseStartOrn=startOr,
baseConstraint="static",
physicsClient=physicsClient,
) # todo: something looks funny for constrained and free baseConstraint
manipulators.append(manipulator)
return manipulators
import pybullet_data
import numpy as np
import os
import sys
import time
path = os.path.abspath(
os.path.join(os.path.dirname(__file__), "..", "..", ".."))
sys.path.insert(0, path)
from somo.sm_manipulator_definition import SMManipulatorDefinition
from somo.create_cmassembly_urdf import create_cmassembly_urdf
from somo.sm_link_definition import SMLinkDefinition
# load manipulator definition
manipulator_definition = SMManipulatorDefinition.from_file("finger_def.yaml")
# offset_0= [0, 0, 0, np.pi/4, 0, 0]
# offset_1= [1, 0, 0, np.pi/4, 0, 0]
# offset_2= [1, 1, 0, np.pi/4, 0, np.pi]
# offset_3= [0, 1, 0, np.pi/4, 0, np.pi]
offset_0 = [1, 0, 1, np.pi / 3, 0, 0]
offset_1 = [0, 1, 1, 0, 0, 0]
offset_2 = [1, 1, 1, np.pi / 3, 0, np.pi / 3]
offset_3 = [0, 1, 1, -np.pi / 3, -np.pi / 3, np.pi / 3]
# offset_0= [1, 0, 1, 0*np.pi/4, 0, 0]
# offset_1= [0, 0, 1, 0*np.pi/4, 0, 0]
# offset_2= [1, 1, 1, 0*np.pi/4, 0, 0*np.pi]
# offset_3= [0, 1, 1, 0*np.pi/4, 0, 0*np.pi]
# create the urdf
def manipulators_from_links(link_definition_dict_1, link_definition_dict_2):
"""
helper fn to quickly create a few manipulators with varying joint, base, and tip definitions from two link definitions
"""
joint_definition_dict_1 = {
"joint_type": "revolute",
"axis": [1, 0, 0],
"limits": [
-np.pi,
np.pi,
100,
3,
], # TODO: distinguish between hard and soft joint limits
"spring_stiffness": 1000,
"joint_neutral_position": 0,
"neutral_axis_offset": [0, 0.05, 0, 0.0, 0.0, 0.0],
# todo: adjust joint definition; this should also be normal to the joint axis. add check to rule out non-zero angle offsets (not implemented)
"joint_control_limit_force": 2.0,
}
joint_definition_dict_2 = {
"joint_type": "revolute",
"axis": [0, 1, 0],
"limits": [-np.pi, np.pi, 100, 3],
"spring_stiffness": 1000,
"joint_neutral_position": 0,
"joint_control_limit_force": 2.0,
}
actuator_definition_dict1 = {
"actuator_length": 0.5,
"n_segments": 10,
"link_definition": link_definition_dict_1,
"joint_definitions": [joint_definition_dict_1],
"planar_flag": 1,
}
actuator_definition_dict2 = {
"actuator_length": 1.5,
"n_segments": 10,
"link_definition": link_definition_dict_2,
"joint_definitions":
[joint_definition_dict_1, joint_definition_dict_2],
"planar_flag": 0,
}
tip_definition_dict_sphere = {
"shape_type": "sphere",
"dimensions": [0.15],
"mass": 0.350,
"inertial_values": [1, 0, 0, 1, 0, 1],
"material_color": [0.8, 0.8, 0.8, 1.0],
"material_name": "white",
"origin_offset": [0, -0.1, -0.3, 0, 0, 0],
}
base_definition = {
"shape_type": "box",
"dimensions": [0.2, 0.2, 0.3],
"mass": 1.350,
"inertial_values": [1, 0, 0, 1, 0, 1],
"material_color": [0.1, 0.8, 0.1, 1.0],
"material_name": "green",
}
manipulator_definitions = [
SMManipulatorDefinition(
n_act=2,
base_definition=None,
actuator_definitions=[
actuator_definition_dict1, actuator_definition_dict2
],
tip_definition=None,
manipulator_name="test1",
urdf_filename=TEST_UDRF_PATH,
),
SMManipulatorDefinition(
n_act=2,
base_definition=base_definition,
actuator_definitions=[
actuator_definition_dict1, actuator_definition_dict2
],
tip_definition=tip_definition_dict_sphere,
manipulator_name="test2",
urdf_filename=TEST_UDRF_PATH,
),
]
return manipulator_definitions
def manipulator_actuation_tester(
gui: bool = False, total_sim_steps=1000, num_axes=None
):
"""
tests whether a manipulator can be instantiated in pybullet and whether a sinusoidal torque actuation can be applied
"""
palmBaseHeight = 0.05
startPositions = [0, 0, palmBaseHeight]
startOrientations = p.getQuaternionFromEuler([0, 0, 0])
# load the manipulator definition
manipulater_def_path = os.path.join(
Path(__file__).parent, "manipulator_test_def.yaml"
)
with open(manipulater_def_path, "r") as manipulater_def_file:
manipulater_def_dict_template = yaml.safe_load(manipulater_def_file)
manipulater_def_dict = copy.deepcopy(manipulater_def_dict_template)
# if num_axes input is None, use the template directly.
if not num_axes:
pass
else: # if num_axes is provided, overwrite the number of axes that are actually instantiated
# import pdb; pdb.set_trace()
manipulater_def_dict["actuator_definitions"][0][
"joint_definitions"
] = manipulater_def_dict_template["actuator_definitions"][0][
"joint_definitions"
][
:num_axes
]
if (
num_axes == 1
): # if this is 1, we have a planar actuator in the test and need to set the planar_flag accordingly
manipulater_def_dict["actuator_definitions"][0]["planar_flag"] = 1
manipulator_def = SMManipulatorDefinition(**manipulater_def_dict)
# instantiate manipulator
manipulator = SMContinuumManipulator(manipulator_def)
if gui:
physicsClient = p.connect(p.GUI)
else:
physicsClient = p.connect(p.DIRECT)
# setting up the physics client
p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(enableConeFriction=1)
p.setRealTimeSimulation(
0
) # only if this is set to 0 and the simulation is done with explicit steps will the torque control work correctly
p.setPhysicsEngineParameter(
enableFileCaching=0
) # x todo: check if this makes things faster
bullet_time_step = 0.0001
p.setTimeStep(bullet_time_step)
# load the ground plane into pybullet
p.setAdditionalSearchPath(
pybullet_data.getDataPath()
) # defines the path used by p.loadURDF
# _planeId = p.loadURDF("plane.urdf")
manipulator.load_to_pybullet(
baseStartPos=startPositions,
baseStartOrn=startOrientations,
baseConstraint="static",
physicsClient=physicsClient,
) # todo: something looks funny for constrained and free baseConstraint
p.changeDynamics(manipulator.bodyUniqueId, -1, lateralFriction=2)
p.changeDynamics(manipulator.bodyUniqueId, -1, restitution=1)
for axis_nr in range(num_axes):
for i in range(int(total_sim_steps / num_axes)):
action = 50 * np.sin(0.00005 * i)
manipulator.apply_actuation_torques(
actuator_nrs=[0],
axis_nrs=[axis_nr],
actuation_torques=[action],
)
p.stepSimulation(physicsClient)
p.disconnect(physicsClient)
# delete the test urdf
os.remove(manipulator.manipulator_definition.urdf_filename)
# load the bball
ballStartPos = [3.5, 0.75, 0.5]
ballStartOr = p.getQuaternionFromEuler([0, 0, 0])
ballId = p.loadURDF(
"additional_urdfs/bball_court/ball.urdf",
ballStartPos,
ballStartOr,
flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL,
globalScaling=0.5,
)
p.changeDynamics(ballId, -1, lateralFriction=1) # set ball friction
### Create and load the manipulator / arm
# load the manipulator definition
arm_manipulator_def = SMManipulatorDefinition.from_file(
"definitions/bb_arm.yaml")
# create the arm manipulator...
arm = SMContinuumManipulator(arm_manipulator_def)
# ... and load it
startPos = [0, 0, 0]
startOr = p.getQuaternionFromEuler([0, 0, 0])
arm.load_to_pybullet(
baseStartPos=startPos,
baseStartOrn=startOr,
baseConstraint=
"static", # other options are free and constrained, but those are not recommended rn
physicsClient=physicsClient,
)
# below is an example of how lateral friction and restitution can be changed for the whole manipulator.