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.