From bd079c9cb7f1c923900962c4ab3fc63c1e54a372 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Sat, 23 Mar 2019 03:35:23 +0100 Subject: [PATCH] major refactoring to be consistent with PEP8 --- .../trajectory_reproduction_kuka_dmp.py | 2 +- examples/robots/aibo.py | 8 +- examples/robots/allegrohand.py | 12 +- examples/robots/ant.py | 8 +- examples/robots/atlas.py | 6 +- examples/robots/ballbot.py | 4 +- examples/robots/baxter.py | 6 +- examples/robots/bb8.py | 4 +- examples/robots/cartpole.py | 10 +- examples/robots/cassie.py | 8 +- examples/robots/centauro.py | 12 +- examples/robots/cogimon.py | 6 +- examples/robots/coman.py | 20 +- examples/robots/crab.py | 6 +- examples/robots/cubli.py | 15 +- examples/robots/darwin.py | 8 +- examples/robots/edo.py | 4 +- examples/robots/epuck.py | 8 +- examples/robots/f10_racecar.py | 8 +- examples/robots/fetch.py | 6 +- examples/robots/franka.py | 8 +- examples/robots/half_cheetah.py | 2 +- examples/robots/hopper.py | 2 +- examples/robots/hubo.py | 6 +- examples/robots/humanoid.py | 6 +- examples/robots/husky.py | 8 +- examples/robots/hyq.py | 10 +- examples/robots/hyq2max.py | 12 +- examples/robots/jaco.py | 2 +- examples/robots/kr5.py | 4 +- examples/robots/kuka_iiwa.py | 24 +- examples/robots/kuka_lwr.py | 4 +- examples/robots/laikago.py | 8 +- examples/robots/littledog.py | 10 +- examples/robots/load_robot.py | 6 +- examples/robots/manipulator2d.py | 10 +- examples/robots/minitaur.py | 10 +- examples/robots/mkz.py | 8 +- examples/robots/morphex.py | 2 +- examples/robots/nao.py | 6 +- examples/robots/opendog.py | 6 +- examples/robots/pepper.py | 8 +- examples/robots/phantomx.py | 2 +- examples/robots/pleurobot.py | 10 +- examples/robots/pr2.py | 6 +- examples/robots/quadcopter.py | 4 +- examples/robots/rhex.py | 6 +- examples/robots/rrbot.py | 98 +- examples/robots/sawyer.py | 6 +- examples/robots/sea_hexapod.py | 2 +- examples/robots/sea_snake.py | 2 +- examples/robots/softhand.py | 20 +- examples/robots/swimmer.py | 2 +- examples/robots/walker2d.py | 2 +- examples/robots/walkman.py | 14 +- examples/robots/wam.py | 24 +- examples/robots/youbot.py | 14 +- .../actions/robot_actions/joint_actions.py | 28 +- .../actions/robot_actions/link_actions.py | 8 +- .../actions/robot_actions/robot_actions.py | 8 +- pyrobolearn/rewards/__init__.py | 12 +- pyrobolearn/rewards/basic_rewards.py | 182 ++ pyrobolearn/rewards/cost.py | 53 +- pyrobolearn/rewards/gym_reward.py | 22 + pyrobolearn/rewards/reward.py | 166 +- pyrobolearn/rewards/reward_processor.py | 284 ++ pyrobolearn/robots/aibo.py | 36 +- pyrobolearn/robots/allegrohand.py | 34 +- pyrobolearn/robots/ant.py | 36 +- pyrobolearn/robots/atlas.py | 44 +- pyrobolearn/robots/ballbot.py | 28 +- pyrobolearn/robots/base.py | 80 +- pyrobolearn/robots/baxter.py | 38 +- pyrobolearn/robots/bb8.py | 26 +- pyrobolearn/robots/blackbird.py | 42 +- pyrobolearn/robots/cartpole.py | 322 +- pyrobolearn/robots/cassie.py | 42 +- pyrobolearn/robots/centauro.py | 42 +- pyrobolearn/robots/cogimon.py | 50 +- pyrobolearn/robots/coman.py | 62 +- pyrobolearn/robots/crab.py | 36 +- pyrobolearn/robots/cubli.py | 38 +- pyrobolearn/robots/darwin.py | 50 +- pyrobolearn/robots/edo.py | 30 +- pyrobolearn/robots/epuck.py | 36 +- pyrobolearn/robots/f10_racecar.py | 42 +- pyrobolearn/robots/fetch.py | 32 +- pyrobolearn/robots/franka.py | 36 +- pyrobolearn/robots/half_cheetah.py | 28 +- pyrobolearn/robots/hand.py | 20 +- pyrobolearn/robots/hopper.py | 28 +- pyrobolearn/robots/hubo.py | 48 +- pyrobolearn/robots/humanoid.py | 44 +- pyrobolearn/robots/husky.py | 36 +- pyrobolearn/robots/hyq.py | 40 +- pyrobolearn/robots/hyq2max.py | 46 +- pyrobolearn/robots/icub.py | 46 +- pyrobolearn/robots/jaco.py | 28 +- pyrobolearn/robots/kr5.py | 30 +- pyrobolearn/robots/kuka_iiwa.py | 50 +- pyrobolearn/robots/kuka_lwr.py | 30 +- pyrobolearn/robots/laikago.py | 42 +- pyrobolearn/robots/legged_robot.py | 338 +- pyrobolearn/robots/littledog.py | 42 +- pyrobolearn/robots/manipulator.py | 128 +- pyrobolearn/robots/manipulator2d.py | 36 +- pyrobolearn/robots/minitaur.py | 101 +- pyrobolearn/robots/mkz.py | 42 +- pyrobolearn/robots/morphex.py | 28 +- pyrobolearn/robots/nao.py | 46 +- pyrobolearn/robots/opendog.py | 36 +- pyrobolearn/robots/pepper.py | 48 +- pyrobolearn/robots/phantomx.py | 28 +- pyrobolearn/robots/pleurobot.py | 32 +- pyrobolearn/robots/pr2.py | 32 +- pyrobolearn/robots/quadcopter.py | 46 +- pyrobolearn/robots/rhex.py | 38 +- pyrobolearn/robots/robot.py | 2780 +++++++---------- pyrobolearn/robots/rrbot.py | 120 +- pyrobolearn/robots/sawyer.py | 38 +- pyrobolearn/robots/sea_hexapod.py | 28 +- pyrobolearn/robots/sea_snake.py | 28 +- pyrobolearn/robots/softhand.py | 48 +- pyrobolearn/robots/swimmer.py | 28 +- pyrobolearn/robots/uav.py | 15 +- pyrobolearn/robots/usv.py | 5 +- pyrobolearn/robots/uuv.py | 5 +- pyrobolearn/robots/walker2d.py | 28 +- pyrobolearn/robots/walkman.py | 54 +- pyrobolearn/robots/wam.py | 52 +- pyrobolearn/robots/wheeled_robot.py | 67 +- pyrobolearn/robots/youbot.py | 124 +- pyrobolearn/simulators/__init__.py | 28 +- pyrobolearn/simulators/bullet.py | 462 +-- pyrobolearn/simulators/simulator.py | 342 +- pyrobolearn/states/basic_states.py | 6 +- pyrobolearn/states/body_states.py | 76 +- .../states/robot_states/joint_states.py | 12 +- .../states/robot_states/link_states.py | 16 +- .../states/robot_states/robot_states.py | 10 +- .../states/robot_states/sensor_states.py | 4 +- pyrobolearn/states/state_processor.py | 304 ++ .../audio/robots/bridge_speech_wheeled.py | 12 +- .../robots/bridge_controller_wheeled.py | 4 +- .../bridge_mousekeyboard_world.py | 95 +- .../mouse_keyboard/mousekeyboard.py | 6 +- pyrobolearn/tools/interfaces/vr/oculus.py | 38 +- pyrobolearn/utils/__init__.py | 56 +- pyrobolearn/utils/orientation.py | 46 +- pyrobolearn/worlds/world.py | 1175 +++---- 150 files changed, 5289 insertions(+), 4840 deletions(-) create mode 100644 pyrobolearn/rewards/basic_rewards.py create mode 100644 pyrobolearn/rewards/gym_reward.py create mode 100644 pyrobolearn/rewards/reward_processor.py create mode 100644 pyrobolearn/states/state_processor.py diff --git a/examples/imitation/trajectory_reproduction_kuka_dmp.py b/examples/imitation/trajectory_reproduction_kuka_dmp.py index 621e507..04436bb 100644 --- a/examples/imitation/trajectory_reproduction_kuka_dmp.py +++ b/examples/imitation/trajectory_reproduction_kuka_dmp.py @@ -35,7 +35,7 @@ sim = BulletSim() world = BasicWorld(sim) # load robot in the world -robot = world.loadRobot(KukaIIWA) +robot = world.load_robot(KukaIIWA) print("Robot's actuated joint ids: {}".format(robot.joints)) # create state/action diff --git a/examples/robots/aibo.py b/examples/robots/aibo.py index c07acc6..33ebdff 100644 --- a/examples/robots/aibo.py +++ b/examples/robots/aibo.py @@ -14,15 +14,15 @@ sim = BulletSim() world = BasicWorld(sim) # create robot -robot = Aibo(sim) # , useFixedBase=True) +robot = Aibo(sim) # , fixed_base=True) # print information about the robot -robot.printRobotInfo() +robot.print_info() # # Position control using sliders -# robot.addJointSlider(robot.getLeftFrontLegIds() + robot.getRightFrontLegIds()) +# robot.add_joint_slider(robot.left_front_leg + robot.right_front_leg) # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/allegrohand.py b/examples/robots/allegrohand.py index 52d2274..c91507e 100644 --- a/examples/robots/allegrohand.py +++ b/examples/robots/allegrohand.py @@ -14,19 +14,19 @@ sim = BulletSim() world = BasicWorld(sim) # create robot -right_hand = AllegroHand(sim) # , init_pos=(0.,0.,0.), init_orient=(0,0,1,0)) +right_hand = AllegroHand(sim) # , position=(0.,0.,0.), orientation=(0,0,1,0)) # print information about the robot -right_hand.printRobotInfo() -# H = right_hand.calculateMassMatrix() +right_hand.print_info() +# H = right_hand.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) # Position control using sliders -right_hand.addJointSlider() +right_hand.add_joint_slider() for i in count(): - right_hand.updateJointSlider() - # right_hand.setJointPositions([0.] * right_hand.getNumberOfDoFs()) + right_hand.update_joint_slider() + # right_hand.set_joint_positions([0.] * right_hand.num_dofs) # step in simulation world.step(sleep_dt=1./240) diff --git a/examples/robots/ant.py b/examples/robots/ant.py index ccea860..8147102 100644 --- a/examples/robots/ant.py +++ b/examples/robots/ant.py @@ -14,15 +14,15 @@ sim = BulletSim() world = BasicWorld(sim) # create robot -robot = Ant(sim) # , useFixedBase=True) +robot = Ant(sim) # , fixed_base=True) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider(robot.getLeftFrontLegIds() + robot.getRightFrontLegIds()) +# robot.add_joint_slider(robot.left_front_leg + robot.right_front_leg) # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/atlas.py b/examples/robots/atlas.py index b446491..a82c71d 100644 --- a/examples/robots/atlas.py +++ b/examples/robots/atlas.py @@ -17,12 +17,12 @@ world = BasicWorld(sim) robot = Atlas(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # position control using sliders -robot.addJointSlider(robot.getLeftLegIds()) +robot.add_joint_slider(robot.left_leg) # run simulator for _ in count(): - robot.updateJointSlider() + robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/ballbot.py b/examples/robots/ballbot.py index 5f30a1b..c9baeb3 100644 --- a/examples/robots/ballbot.py +++ b/examples/robots/ballbot.py @@ -17,10 +17,10 @@ world = BasicWorld(sim) robot = Ballbot(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() for i in count(): - # robot.setJointVelocities([0, -1, 0]) + # robot.set_joint_velocities([0, -1, 0]) # step in simulation world.step(sleep_dt=1./240) diff --git a/examples/robots/baxter.py b/examples/robots/baxter.py index 2dd6cb7..1fedbed 100644 --- a/examples/robots/baxter.py +++ b/examples/robots/baxter.py @@ -17,12 +17,12 @@ world = BasicWorld(sim) robot = Baxter(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/bb8.py b/examples/robots/bb8.py index d48fd4e..b1b5546 100644 --- a/examples/robots/bb8.py +++ b/examples/robots/bb8.py @@ -17,10 +17,10 @@ world = BasicWorld(sim) robot = BB8(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() for i in count(): - robot.setJointVelocities([0, -1, 0]) + robot.set_joint_velocities([0, -1, 0]) # step in simulation world.step(sleep_dt=1./240) diff --git a/examples/robots/cartpole.py b/examples/robots/cartpole.py index 4fb011c..423bd20 100644 --- a/examples/robots/cartpole.py +++ b/examples/robots/cartpole.py @@ -100,12 +100,12 @@ if __name__ == "__main__": robot = CartPole(sim, num_links=num_links) # print information about the robot - robot.printRobotInfo() + robot.print_info() - robot.getSymbolicEquationsOfMotion() + robot.get_symbolic_equations_of_motion() eq_point = np.zeros((num_links + 1) * 2) # state = [q, dq] - A, B = robot.linearizeEquationOfMotion(eq_point) + A, B = robot.linearize_equations_of_motion(eq_point) # LQR controller lqr = LQR(A, B) @@ -113,9 +113,9 @@ if __name__ == "__main__": for i in count(): # control - x = np.concatenate((robot.getJointPositions(), robot.getJointVelocities())) + x = np.concatenate((robot.get_joint_positions(), robot.get_joint_velocities())) u = K.dot(eq_point - x) - robot.setJointTorques(u[0], 0) + robot.set_joint_torques(u[0], 0) print("U[0] = {}".format(u[0])) diff --git a/examples/robots/cassie.py b/examples/robots/cassie.py index 5d000cc..92c83af 100644 --- a/examples/robots/cassie.py +++ b/examples/robots/cassie.py @@ -17,13 +17,13 @@ world = BasicWorld(sim) robot = Cassie(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - robot.moveJointHomePositions() + # robot.update_joint_slider() + robot.move_joint_home_positions() world.step(sleep_dt=1./240) diff --git a/examples/robots/centauro.py b/examples/robots/centauro.py index 233cda9..2b9024a 100644 --- a/examples/robots/centauro.py +++ b/examples/robots/centauro.py @@ -14,17 +14,17 @@ sim = BulletSim() world = BasicWorld(sim) # load robot -robot = Centauro(sim) # , useFixedBase=True) +robot = Centauro(sim) # , fixed_base=True) # print information about the robot -robot.printRobotInfo() -print("Number of Legs: {}".format(robot.getNumberOfLegs())) -print("Number of Arms: {}".format(robot.getNumberOfArms())) +robot.print_info() +print("Number of Legs: {}".format(robot.num_legs)) +print("Number of Arms: {}".format(robot.num_arms)) -# robot.addJointSlider(robot.getRightFrontLegIds()) +# robot.add_joint_slider(robot.getRightFrontLegIds()) robot.drive(speed=3) # run simulator for _ in count(): - robot.updateJointSlider() + robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/cogimon.py b/examples/robots/cogimon.py index aa27c35..8761d06 100644 --- a/examples/robots/cogimon.py +++ b/examples/robots/cogimon.py @@ -17,12 +17,12 @@ world = BasicWorld(sim) robot = Cogimon(sim, lower_body=False) # print information about the robot -robot.printRobotInfo() +robot.print_info() # # Position control using sliders -robot.addJointSlider(robot.left_leg) +robot.add_joint_slider(robot.left_leg) # run simulator for _ in count(): - robot.updateJointSlider() + robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/coman.py b/examples/robots/coman.py index 772bd81..a853b93 100644 --- a/examples/robots/coman.py +++ b/examples/robots/coman.py @@ -14,23 +14,23 @@ sim = BulletSim() world = BasicWorld(sim) # create robot -robot = Coman(sim, useFixedBase=True) +robot = Coman(sim, fixed_base=True) # print information about the robot -robot.printRobotInfo() +robot.print_info() print(robot.link_names) # # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() -robot.changeTransparency() -# robot.drawLinkCoMs() -robot.drawLinkFrames() -# robot.drawBoundingBoxes(robot.right_leg[4]) +robot.change_transparency() +# robot.draw_link_coms() +robot.draw_link_frames() +# robot.draw_bounding_boxes(robot.right_leg[4]) # run simulator for _ in count(): - # robot.updateJointSlider() - # robot.computeAndDrawCoMPosition() - # robot.computeAndDrawProjectedCoMPosition() + # robot.update_joint_slider() + # robot.compute_and_draw_com_position() + # robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/examples/robots/crab.py b/examples/robots/crab.py index 4e2904f..88a0add 100644 --- a/examples/robots/crab.py +++ b/examples/robots/crab.py @@ -17,13 +17,13 @@ world = BasicWorld(sim) robot = Crab(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -robot.addJointSlider(robot.right_middle_leg) +robot.add_joint_slider(robot.right_middle_leg) # run simulation for i in count(): - robot.updateJointSlider() + robot.update_joint_slider() # step in simulation world.step(sleep_dt=1./240) diff --git a/examples/robots/cubli.py b/examples/robots/cubli.py index a11d715..362aa79 100644 --- a/examples/robots/cubli.py +++ b/examples/robots/cubli.py @@ -4,7 +4,8 @@ import numpy as np from itertools import count -from pyrobolearn.simulators import BulletSim, pybullet +from pyrobolearn.utils.orientation import get_rpy_from_quaternion +from pyrobolearn.simulators import BulletSim from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Cubli @@ -21,8 +22,8 @@ orientation = [0.383, 0, 0, 0.924] robot = Cubli(sim, position, orientation, scaling=scale) # print information about the robot -robot.printRobotInfo() -H = robot.calculateMassMatrix(qIdx=slice(6, 6+len(robot.joints))) # floating base, thus keep only the last q +robot.print_info() +H = robot.get_mass_matrix(q_idx=slice(6, 6 + len(robot.joints))) # floating base, thus keep only the last q print("Inertia matrix: H(q) = {}\n".format(H)) # PD control @@ -32,13 +33,13 @@ desired_roll = np.pi / 4. for i in count(): # get state - quaternion = robot.getBaseOrientation(False) - w = robot.getBaseAngularVelocity() - euler = pybullet.getEulerFromQuaternion(quaternion.tolist()) + quaternion = robot.get_base_orientation() + w = robot.get_base_angular_velocity() + euler = get_rpy_from_quaternion(quaternion) # PD control torques = [-Kp * (desired_roll - euler[0]) + Kd * w[0], 0., 0.] - robot.setJointTorques(torques) + robot.set_joint_torques(torques) # step in simulation world.step(sleep_dt=1./240) diff --git a/examples/robots/darwin.py b/examples/robots/darwin.py index 6e203ce..af420af 100644 --- a/examples/robots/darwin.py +++ b/examples/robots/darwin.py @@ -14,16 +14,16 @@ sim = BulletSim() world = BasicWorld(sim) # create robot -robot = Darwin(sim, useFixedBase=False) +robot = Darwin(sim, fixed_base=False) # print information about the robot -robot.printRobotInfo() +robot.print_info() print(robot.link_names) # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/edo.py b/examples/robots/edo.py index 2ef5bd3..6b7b4e9 100644 --- a/examples/robots/edo.py +++ b/examples/robots/edo.py @@ -17,8 +17,8 @@ world = BasicWorld(sim) robot = Edo(sim) # print information about the robot -robot.printRobotInfo() -# H = robot.calculateMassMatrix() +robot.print_info() +# H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) for i in count(): diff --git a/examples/robots/epuck.py b/examples/robots/epuck.py index 8eb38e8..8ec101d 100644 --- a/examples/robots/epuck.py +++ b/examples/robots/epuck.py @@ -18,18 +18,18 @@ world = BasicWorld(sim) robots = [] for _ in range(5): x, y = np.random.uniform(low=-2, high=2, size=2) - robot = world.loadRobot(Epuck, position=(x, y, 0)) + robot = world.load_robot(Epuck, position=(x, y, 0)) robots.append(robot) # print information about the robot -robots[0].printRobotInfo() +robots[0].print_info() # Position control using sliders -# robots[0].addJointSlider() +# robots[0].add_joint_slider() # run simulator for _ in count(): - # robots[0].updateJointSlider() + # robots[0].update_joint_slider() for robot in robots: robot.drive(5) world.step(sleep_dt=1./240) diff --git a/examples/robots/f10_racecar.py b/examples/robots/f10_racecar.py index 0d9cc31..cf06cf0 100644 --- a/examples/robots/f10_racecar.py +++ b/examples/robots/f10_racecar.py @@ -17,13 +17,13 @@ world = BasicWorld(sim) robot = F10Racecar(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - robot.driveForward(10) + # robot.update_joint_slider() + robot.drive_forward(10) world.step(sleep_dt=1./240) diff --git a/examples/robots/fetch.py b/examples/robots/fetch.py index cfc46ba..ab1bbc7 100644 --- a/examples/robots/fetch.py +++ b/examples/robots/fetch.py @@ -17,12 +17,12 @@ world = BasicWorld(sim) robot = Fetch(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -robot.addJointSlider() +robot.add_joint_slider() # run simulator for _ in count(): - robot.updateJointSlider() + robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/franka.py b/examples/robots/franka.py index bade6b6..e04f5ff 100644 --- a/examples/robots/franka.py +++ b/examples/robots/franka.py @@ -17,14 +17,14 @@ world = BasicWorld(sim) robot = Franka(sim) # print information about the robot -robot.printRobotInfo() -# H = robot.calculateMassMatrix() +robot.print_info() +# H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() for i in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() # step in simulation world.step(sleep_dt=1./240) diff --git a/examples/robots/half_cheetah.py b/examples/robots/half_cheetah.py index 4ed1d2b..f99115d 100644 --- a/examples/robots/half_cheetah.py +++ b/examples/robots/half_cheetah.py @@ -17,7 +17,7 @@ world = BasicWorld(sim) robot = HalfCheetah(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # run simulation for i in count(): diff --git a/examples/robots/hopper.py b/examples/robots/hopper.py index 5c26472..dd17a83 100644 --- a/examples/robots/hopper.py +++ b/examples/robots/hopper.py @@ -17,7 +17,7 @@ world = BasicWorld(sim) robot = Hopper(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # run simulation for i in count(): diff --git a/examples/robots/hubo.py b/examples/robots/hubo.py index b6c3341..1eb633a 100644 --- a/examples/robots/hubo.py +++ b/examples/robots/hubo.py @@ -17,12 +17,12 @@ world = BasicWorld(sim) robot = Hubo(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/humanoid.py b/examples/robots/humanoid.py index aadc1ce..889c1a9 100644 --- a/examples/robots/humanoid.py +++ b/examples/robots/humanoid.py @@ -17,13 +17,13 @@ world = BasicWorld(sim) robot = Humanoid(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulation for i in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() # step in simulation world.step(sleep_dt=1./240) diff --git a/examples/robots/husky.py b/examples/robots/husky.py index 8df769d..68a1ac8 100644 --- a/examples/robots/husky.py +++ b/examples/robots/husky.py @@ -17,13 +17,13 @@ world = BasicWorld(sim) robot = Husky(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - robot.driveForward(2) + # robot.update_joint_slider() + robot.drive_forward(2) world.step(sleep_dt=1./240) diff --git a/examples/robots/hyq.py b/examples/robots/hyq.py index 2364992..633a9b1 100644 --- a/examples/robots/hyq.py +++ b/examples/robots/hyq.py @@ -17,14 +17,14 @@ world = BasicWorld(sim) robot = HyQ(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # # Position control using sliders -robot.addJointSlider(robot.getLeftFrontLegIds()) +robot.add_joint_slider(robot.left_front_leg) # run simulator for _ in count(): - robot.updateJointSlider() - robot.computeAndDrawCoMPosition() - robot.computeAndDrawProjectedCoMPosition() + robot.update_joint_slider() + robot.compute_and_draw_com_position() + robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/examples/robots/hyq2max.py b/examples/robots/hyq2max.py index 036a662..7ed24fc 100644 --- a/examples/robots/hyq2max.py +++ b/examples/robots/hyq2max.py @@ -12,21 +12,21 @@ sim = BulletSim() # create world world = BasicWorld(sim) -world.loadJapaneseMonastery() +world.load_japanese_monastery() # create robot robot = HyQ2Max(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider(robot.getLeftFrontLegIds()) +# robot.add_joint_slider(robot.getLeftFrontLegIds()) # run simulator for _ in count(): - # robot.updateJointSlider() - robot.computeAndDrawCoMPosition() - robot.computeAndDrawProjectedCoMPosition() + # robot.update_joint_slider() + robot.compute_and_draw_com_position() + robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/examples/robots/jaco.py b/examples/robots/jaco.py index 756557d..6d86cea 100644 --- a/examples/robots/jaco.py +++ b/examples/robots/jaco.py @@ -17,7 +17,7 @@ world = BasicWorld(sim) robot = Jaco(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # run simulation for i in count(): diff --git a/examples/robots/kr5.py b/examples/robots/kr5.py index 1183f33..94d39f4 100644 --- a/examples/robots/kr5.py +++ b/examples/robots/kr5.py @@ -17,8 +17,8 @@ world = BasicWorld(sim) robot = KR5(sim) # print information about the robot -robot.printRobotInfo() -# H = robot.calculateMassMatrix() +robot.print_info() +# H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) for i in count(): diff --git a/examples/robots/kuka_iiwa.py b/examples/robots/kuka_iiwa.py index 8e51f00..a2582a0 100644 --- a/examples/robots/kuka_iiwa.py +++ b/examples/robots/kuka_iiwa.py @@ -18,11 +18,11 @@ world = BasicWorld(sim) robot = KukaIIWA(sim) # print information about the robot -robot.printRobotInfo() -# H = robot.calculateMassMatrix() +robot.print_info() +# H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) -# print(robot.getLinkWorldPositions(flatten=False)) +# print(robot.get_link_world_positions(flatten=False)) K = 5000*np.identity(3) # D = 2 * np.sqrt(K) @@ -30,22 +30,22 @@ K = 5000*np.identity(3) D = 100 * np.identity(3) x_des = np.array([0.3, 0.0, 0.8]) x_des = np.array([0.52557296, 0.09732758, 0.80817658]) -linkId = robot.getLinkIds('iiwa_link_ee') +link_id = robot.get_link_ids('iiwa_link_ee') for i in count(): - # print(robot.getLinkWorldPositions(flatten=False)) + # print(robot.get_link_world_positions(flatten=False)) # get state - q = robot.getJointPositions() - dq = robot.getJointVelocities() - x = robot.getLinkWorldPositions(linkId) - dx = robot.getLinkWorldLinearVelocities(linkId) + q = robot.get_joint_positions() + dq = robot.get_joint_velocities() + x = robot.get_link_world_positions(link_id) + dx = robot.get_link_world_linear_velocities(link_id) # get (linear) jacobian - J = robot.getLinearJacobian(linkId, q) + J = robot.get_linear_jacobian(link_id, q) # get coriolis, gravity compensation torques - torques = robot.getCoriolisAndGravityCompensationTorques(q, dq) + torques = robot.get_coriolis_and_gravity_compensation_torques(q, dq) # Impedance control: attractor point F = K.dot(x_des - x) - D.dot(dx) @@ -53,7 +53,7 @@ for i in count(): tau = J.T.dot(F) print(tau) torques += tau - robot.setJointTorques(torques) + robot.set_joint_torques(torques) # step in simulation world.step(sleep_dt=1./240) diff --git a/examples/robots/kuka_lwr.py b/examples/robots/kuka_lwr.py index 7f904f3..696b77c 100644 --- a/examples/robots/kuka_lwr.py +++ b/examples/robots/kuka_lwr.py @@ -17,8 +17,8 @@ world = BasicWorld(sim) robot = KukaLWR(sim) # print information about the robot -robot.printRobotInfo() -# H = robot.calculateMassMatrix() +robot.print_info() +# H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) for i in count(): diff --git a/examples/robots/laikago.py b/examples/robots/laikago.py index 277a23f..12fb442 100644 --- a/examples/robots/laikago.py +++ b/examples/robots/laikago.py @@ -17,13 +17,13 @@ world = BasicWorld(sim) robot = Laikago(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - robot.moveJointHomePositions() + # robot.update_joint_slider() + robot.move_joint_home_positions() world.step(sleep_dt=1./240) diff --git a/examples/robots/littledog.py b/examples/robots/littledog.py index 5e59f6e..4767b59 100644 --- a/examples/robots/littledog.py +++ b/examples/robots/littledog.py @@ -17,14 +17,14 @@ world = BasicWorld(sim) robot = LittleDog(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - # robot.computeAndDrawCoMPosition() - # robot.computeAndDrawProjectedCoMPosition() + # robot.update_joint_slider() + # robot.compute_and_draw_com_position() + # robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/examples/robots/load_robot.py b/examples/robots/load_robot.py index 6baa688..d13c838 100644 --- a/examples/robots/load_robot.py +++ b/examples/robots/load_robot.py @@ -18,11 +18,11 @@ world = BasicWorld(sim) for i, robot_name in enumerate(implemented_robots): if robot_name not in robot_not_working: # instantiate the given robot - robot = world.loadRobot(robot_name) + robot = world.load_robot(robot_name) # print info about the robot print("Robot n{}: {}".format(i+1, robot)) - # robot.printRobotInfo() + # robot.print_info() # run for few moments in the world for t in range(250): @@ -30,4 +30,4 @@ for i, robot_name in enumerate(implemented_robots): world.step(sleep_dt=1./240) # remove the robot from the world - world.removeObject(robot) + world.remove(robot) diff --git a/examples/robots/manipulator2d.py b/examples/robots/manipulator2d.py index 573200f..c881564 100644 --- a/examples/robots/manipulator2d.py +++ b/examples/robots/manipulator2d.py @@ -14,14 +14,14 @@ sim = BulletSim() world = BasicWorld(sim) # create robot -robot = Manipulator2D(sim, init_pos=(0, -0.25, 0)) -robot1 = Manipulator2D(sim, init_pos=(0, 0.25, 0)) -robot.printRobotInfo() +robot = Manipulator2D(sim, position=(0, -0.25, 0)) +robot1 = Manipulator2D(sim, position=(0, 0.25, 0)) +robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/minitaur.py b/examples/robots/minitaur.py index 2b8efb5..11df4f1 100644 --- a/examples/robots/minitaur.py +++ b/examples/robots/minitaur.py @@ -17,14 +17,14 @@ world = BasicWorld(sim) robot = Minitaur(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -robot.addJointSlider(robot.getLeftFrontLegIds()) +# robot.add_joint_slider(robot.left_front_leg) # run simulator for _ in count(): - robot.updateJointSlider() - # robot.computeAndDrawCoMPosition() - # robot.computeAndDrawProjectedCoMPosition() + # robot.update_joint_slider() + # robot.compute_and_draw_com_position() + # robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/examples/robots/mkz.py b/examples/robots/mkz.py index 312bc07..96e6119 100644 --- a/examples/robots/mkz.py +++ b/examples/robots/mkz.py @@ -17,13 +17,13 @@ world = BasicWorld(sim) robot = MKZ(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - robot.driveForward(2) + # robot.update_joint_slider() + robot.drive_forward(2) world.step(sleep_dt=1./240) diff --git a/examples/robots/morphex.py b/examples/robots/morphex.py index d38ddec..2c3a8cf 100644 --- a/examples/robots/morphex.py +++ b/examples/robots/morphex.py @@ -17,7 +17,7 @@ world = BasicWorld(sim) robot = Morphex(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # run simulation for i in count(): diff --git a/examples/robots/nao.py b/examples/robots/nao.py index e1bd4cb..36da221 100644 --- a/examples/robots/nao.py +++ b/examples/robots/nao.py @@ -17,12 +17,12 @@ world = BasicWorld(sim) robot = Nao(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider(robot.getLeftArmIds()) +# robot.add_joint_slider(robot.getLeftArmIds()) # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/opendog.py b/examples/robots/opendog.py index e9d201f..991de4a 100644 --- a/examples/robots/opendog.py +++ b/examples/robots/opendog.py @@ -17,12 +17,12 @@ world = BasicWorld(sim) robot = OpenDog(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider(robot.getLeftFrontLegIds()) +# robot.add_joint_slider(robot.getLeftFrontLegIds()) # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/pepper.py b/examples/robots/pepper.py index c9ab77b..2bc433d 100644 --- a/examples/robots/pepper.py +++ b/examples/robots/pepper.py @@ -18,14 +18,14 @@ world = BasicWorld(sim) robot = Pepper(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for i in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() if i % 20 == 0: - robot.cameraTop.getRGBImage() + robot.camera_top.get_rgb_image() world.step(sleep_dt=1./240) diff --git a/examples/robots/phantomx.py b/examples/robots/phantomx.py index b96b783..4e5ea5e 100644 --- a/examples/robots/phantomx.py +++ b/examples/robots/phantomx.py @@ -17,7 +17,7 @@ world = BasicWorld(sim) robot = PhantomX(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # run simulation for i in count(): diff --git a/examples/robots/pleurobot.py b/examples/robots/pleurobot.py index e151639..16477c5 100644 --- a/examples/robots/pleurobot.py +++ b/examples/robots/pleurobot.py @@ -17,14 +17,14 @@ world = BasicWorld(sim) robot = Pleurobot(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - # robot.computeAndDrawCoMPosition() - # robot.computeAndDrawProjectedCoMPosition() + # robot.update_joint_slider() + # robot.compute_and_draw_com_position() + # robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/examples/robots/pr2.py b/examples/robots/pr2.py index 547fe29..0ae5c64 100644 --- a/examples/robots/pr2.py +++ b/examples/robots/pr2.py @@ -17,12 +17,12 @@ world = BasicWorld(sim) robot = PR2(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/quadcopter.py b/examples/robots/quadcopter.py index be5eda8..5f67540 100644 --- a/examples/robots/quadcopter.py +++ b/examples/robots/quadcopter.py @@ -18,7 +18,7 @@ world = BasicWorld(sim) robot = Quadcopter(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() rpm = robot.getStationaryRPM() print("Stationary RPM: {}".format(rpm)) @@ -27,6 +27,6 @@ v = [v, -v, v, -v] # run simulation for i in count(): - robot.setJointVelocities(v) + robot.set_joint_velocities(v) # step in simulation world.step(sleep_dt=1./240) diff --git a/examples/robots/rhex.py b/examples/robots/rhex.py index 1e050f6..fb14874 100644 --- a/examples/robots/rhex.py +++ b/examples/robots/rhex.py @@ -17,14 +17,14 @@ world = BasicWorld(sim) robot = Rhex(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # Position control using sliders -# robot.addJointSlider(robot.right_back_leg) +# robot.add_joint_slider(robot.right_back_leg) # run simulation for i in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() robot.drive(2) # step in simulation world.step(sleep_dt=1./240) diff --git a/examples/robots/rrbot.py b/examples/robots/rrbot.py index 849c1bd..27095ec 100644 --- a/examples/robots/rrbot.py +++ b/examples/robots/rrbot.py @@ -15,35 +15,35 @@ world = BasicWorld(sim) # load robot robot = RRBot(sim) -# robot.addJointSlider() +# robot.add_joint_slider() print("Robot: {}".format(robot)) -print("Total number of joints: {}".format(robot.getNumberOfJoints())) -print("Joint names: {}".format(robot.getJointNames(range(robot.getNumberOfJoints())))) -print("Link names: {}".format(robot.getLinkNames(range(robot.getNumberOfJoints())))) +print("Total number of joints: {}".format(robot.num_joints)) +print("Joint names: {}".format(robot.get_joint_names(range(robot.num_joints)))) +print("Link names: {}".format(robot.get_link_names(range(robot.num_joints)))) -print("Number of DoFs: {}".format(robot.getNumberOfDoFs())) +print("Number of DoFs: {}".format(robot.num_dofs)) print("Robot actuated joint ids: {}".format(robot.joints)) -print("Actuated joint names: {}".format(robot.getJointNames())) -print("Actuated link names: {}".format(robot.getLinkNames())) -print("Current joint positions: {}".format(robot.getJointPositions())) +print("Actuated joint names: {}".format(robot.get_joint_names())) +print("Actuated link names: {}".format(robot.get_link_names())) +print("Current joint positions: {}".format(robot.get_joint_positions())) -print("Number of end-effectors: {}".format(robot.getNumberOfEndEffectors())) -print("End-effector names: {}".format(robot.getEndEffectorNames())) +print("Number of end-effectors: {}".format(robot.num_end_effectors)) +print("End-effector names: {}".format(robot.get_link_names(robot.end_effectors))) -print("Robot base position: {}".format(robot.getBasePosition())) -robot.changeTransparency() -visuals = robot.sim.getVisualShapeData(robot.id) +print("Robot base position: {}".format(robot.get_base_position())) +robot.change_transparency() +visuals = robot.sim.get_visual_shape_data(robot.id) visuals = {visual[1]: visual[3] for visual in visuals} -# robot.drawLinkCoMs() -robot.drawLinkFrames() -# robot.drawBoundingBoxes() +# robot.draw_link_coms() +robot.draw_link_frames() +# robot.draw_bounding_boxes() for i in robot.joints: print("Link {}".format(i)) - state = robot.sim.getLinkState(robot.id, i) - info = robot.sim.getJointInfo(robot.id, i) + state = robot.sim.get_link_state(robot.id, i) + info = robot.sim.get_joint_info(robot.id, i) print("\t CoM world position: {}".format(state[0])) print("\t Local inertial frame position: {}".format(state[2])) @@ -62,71 +62,71 @@ for _ in count(): raw_input('press enter') -print("Inertia matrix: {}".format(np.array(sim.calculateMassMatrix(robot.id, [0.,0.,0.,0.,0.,0.])))) +print("Inertia matrix: {}".format(np.array(sim.calculate_mass_matrix(robot.id, [0., 0., 0., 0., 0., 0.])))) linkId = 2 -com_frame = robot.getLinkStates(linkId)[2] -q = robot.getJointPositions() +com_frame = robot.get_link_states(linkId)[2] +q = robot.get_joint_positions() print(com_frame) # com_frame = [0.,0.,0.] -print("Jacobian matrix: {}".format(np.vstack((sim.calculateJacobian(robot.id, linkId, com_frame, q.tolist(), [0.,0.], [0.,0.]))))) +print("Jacobian matrix: {}".format(sim.calculate_jacobian(robot.id, linkId, com_frame))) -Jlin = robot.calculateJacobian(linkId+1, localPosition=(0.,0.,0.))[:3] +Jlin = robot.get_jacobian(linkId + 1)[:3] print("Jacobian matrix: {}".format(Jlin)) -robot.drawVelocityManipulabilityEllipsoid(linkId+1, Jlin) +robot.draw_velocity_manipulability_ellipsoid(linkId + 1, Jlin) -Jlin = robot.calculateJacobian(linkId)[:3] -robot.drawVelocityManipulabilityEllipsoid(linkId, Jlin, color=(1,0,0,0.7)) +Jlin = robot.get_jacobian(linkId)[:3] +robot.draw_velocity_manipulability_ellipsoid(linkId, Jlin, color=(1, 0, 0, 0.7)) cnt = 0 for i in count(): if i%240 == 0: if cnt < 3: - Jlin = robot.calculateJacobian(linkId + 1, localPosition=(0., 0., 0.))[:3] - robot.drawVelocityManipulabilityEllipsoid(linkId + 1, Jlin) + Jlin = robot.get_jacobian(linkId + 1)[:3] + robot.draw_velocity_manipulability_ellipsoid(linkId + 1, Jlin) cnt += 1 world.step(sleep_dt=1./240) - # robot.setJointTorques() + # robot.set_joint_torques() raw_input('press enter') -print(robot.getLinkNames()) +print(robot.get_link_names()) force = np.array([1., 0., 0.]) pos = np.array([0., 0., 0.]) -sim.applyExternalForce(robot.id, 1, force, pos, flags=p.LINK_FRAME) # link_frame = 1 +sim.apply_external_force(robot.id, 1, force, pos, flags=p.LINK_FRAME) # link_frame = 1 -slider = sim.addUserDebugParameter('force', -1000., 1000., 0) +slider = sim.add_user_debug_parameter('force', -1000., 1000., 0) dq, ddq = [0., 0.], [0., 0.] -J = sim.calculateJacobian(robot.id, 1, [0.,0.,0.], [0.,0.], dq, ddq) +J = sim.calculate_jacobian(robot.id, 1, [0., 0., 0.]) print(np.array(J[0])) -a = robot.getJointPositions() +a = robot.get_joint_positions() # print(robot.getJacobianMatrix(1, np.array([0.,0.]))) # TODO: need to convert numpy array to list linkId = 2 -com_frame = robot.getLinkStates(linkId)[2] -xdes = np.array(robot.getLinkWorldPositions(linkId)) +com_frame = robot.get_link_states(linkId)[2] +xdes = np.array(robot.get_link_world_positions(linkId)) K = 100*np.identity(3) D = 2*np.sqrt(K) # critically damped D = 3*D # manually increase damping # run simulator for i in range(10000): - joint_states = p.getJointStates(robot.id, robot.joints) + joint_states = p.get_joint_states(robot.id, robot.joints) # print("joint state: ", joint_states) q = [joint_state[0] for joint_state in joint_states] dq = [joint_state[1] for joint_state in joint_states] - # q = robot.getJointPositions().tolist() - # dq = robot.getJointVelocities().tolist() - x = np.array(robot.getLinkWorldPositions(linkId)) - dx = np.array(robot.getLinkWorldLinearVelocities(linkId)) - tau = robot.calculateID(q, dq, ddq) # Coriolis, centrifugal and gravity compensation - Jlin = np.array(sim.calculateJacobian(robot.id, linkId, com_frame, q, [0.,0.], ddq)[0]) - F = K.dot(xdes - x) - D.dot(dx) # compute cartesian forces + # q = robot.get_joint_positions().tolist() + # dq = robot.get_joint_velocities().tolist() + x = np.array(robot.get_link_world_positions(linkId)) + dx = np.array(robot.get_link_world_linear_velocities(linkId)) + tau = robot.calculate_inverse_dynamics(ddq, dq, q) # Coriolis, centrifugal and gravity compensation + Jlin = np.array(sim.calculate_jacobian(robot.id, linkId, com_frame)[0]) + F = K.dot(xdes - x) - D.dot(dx) # compute cartesian forces # print("force: {}".format(F)) - tau += Jlin.T.dot(F) # cartesian PD with gravity compensation + tau += Jlin.T.dot(F) # cartesian PD with gravity compensation # tau += Jlin.T.dot(- D.dot(dx)) # active compliance # tau = Jlin.T.dot(F) @@ -137,14 +137,14 @@ for i in range(10000): # print("manipulability: {}".format(w)) # Impedance/Torque control - sim.setJointMotorControlArray(robot.id, robot.joint_indices, sim.TORQUE_CONTROL, forces=tau) + sim.set_joint_motor_control(robot.id, robot.joints, sim.TORQUE_CONTROL, forces=tau) - force = sim.readUserDebugParameter(slider) + force = sim.read_user_debug_parameter(slider) if force > 0: force = np.array([0., 0., 1.]) else: force = np.array([0., 0., 0.]) - sim.applyExternalForce(robot.id, linkId, force, pos, flags=p.LINK_FRAME) # p.LINK_FRAME = 1 + sim.apply_external_force(robot.id, linkId, force, pos, flags=p.LINK_FRAME) # p.LINK_FRAME = 1 - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/sawyer.py b/examples/robots/sawyer.py index 2293152..b47e062 100644 --- a/examples/robots/sawyer.py +++ b/examples/robots/sawyer.py @@ -17,12 +17,12 @@ world = BasicWorld(sim) robot = Sawyer(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # # Position control using sliders -robot.addJointSlider() +robot.add_joint_slider() # run simulator for _ in count(): - robot.updateJointSlider() + robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/examples/robots/sea_hexapod.py b/examples/robots/sea_hexapod.py index 17a34af..37ee317 100644 --- a/examples/robots/sea_hexapod.py +++ b/examples/robots/sea_hexapod.py @@ -17,7 +17,7 @@ world = BasicWorld(sim) robot = SEAHexapod(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # run simulation for i in count(): diff --git a/examples/robots/sea_snake.py b/examples/robots/sea_snake.py index 0d4ac78..042a8df 100644 --- a/examples/robots/sea_snake.py +++ b/examples/robots/sea_snake.py @@ -17,7 +17,7 @@ world = BasicWorld(sim) robot = SEASnake(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # run simulation for i in count(): diff --git a/examples/robots/softhand.py b/examples/robots/softhand.py index 5c63c27..7f8d171 100644 --- a/examples/robots/softhand.py +++ b/examples/robots/softhand.py @@ -14,23 +14,23 @@ sim = BulletSim() world = BasicWorld(sim) # create robot -left_hand = SoftHand(sim, init_pos=(-0.15, 0, 0), left=True) -right_hand = SoftHand(sim, init_pos=(0.15, 0., 0.), init_orient=(0, 0, 1, 0), left=False) +left_hand = SoftHand(sim, position=(-0.15, 0, 0), left=True) +right_hand = SoftHand(sim, position=(0.15, 0., 0.), orientation=(0, 0, 1, 0), left=False) # print information about the robot -left_hand.printRobotInfo() -# H = left_hand.calculateMassMatrix() +left_hand.print_info() +# H = left_hand.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) # Position control using sliders -# left_hand.addJointSlider() +# left_hand.add_joint_slider() -left_hand.setJointPositions([0.] * left_hand.getNumberOfDoFs()) -right_hand.setJointPositions([0.] * right_hand.getNumberOfDoFs()) +left_hand.set_joint_positions([0.] * left_hand.num_dofs) +right_hand.set_joint_positions([0.] * right_hand.num_dofs) for i in count(): - # left_hand.updateJointSlider() - # left_hand.setJointPositions([0.] * left_hand.getNumberOfDoFs()) - # right_hand.setJointPositions([0.] * right_hand.getNumberOfDoFs()) + # left_hand.update_joint_slider() + # left_hand.set_joint_positions([0.] * left_hand.getNumberOfDoFs()) + # right_hand.set_joint_positions([0.] * right_hand.getNumberOfDoFs()) world.step(sleep_dt=1./240) diff --git a/examples/robots/swimmer.py b/examples/robots/swimmer.py index 56f977f..5668c4a 100644 --- a/examples/robots/swimmer.py +++ b/examples/robots/swimmer.py @@ -17,7 +17,7 @@ world = BasicWorld(sim) robot = Swimmer(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # run simulation for i in count(): diff --git a/examples/robots/walker2d.py b/examples/robots/walker2d.py index 3a30bd1..95b408b 100644 --- a/examples/robots/walker2d.py +++ b/examples/robots/walker2d.py @@ -17,7 +17,7 @@ world = BasicWorld(sim) robot = Walker2D(sim) # print information about the robot -robot.printRobotInfo() +robot.print_info() # run simulation for i in count(): diff --git a/examples/robots/walkman.py b/examples/robots/walkman.py index 13a2d43..c05d76c 100644 --- a/examples/robots/walkman.py +++ b/examples/robots/walkman.py @@ -12,22 +12,22 @@ sim = BulletSim() # Create world world = BasicWorld(sim) -world.loadSphere([2., 0, 2.], mass=0., color=(1, 0, 0, 1)) -world.loadSphere([2., 1., 2.], mass=0., color=(0, 0, 1, 1)) +world.load_sphere([2., 0, 2.], mass=0., color=(1, 0, 0, 1)) +world.load_sphere([2., 1., 2.], mass=0., color=(0, 0, 1, 1)) # load robot -robot = Walkman(sim, useFixedBase=False, lower_body=False) +robot = Walkman(sim, fixed_base=False, lower_body=False) # print information about the robot -robot.printRobotInfo() +robot.print_info() # # Position control using sliders -robot.addJointSlider(robot.left_leg) +robot.add_joint_slider(robot.left_leg) # run simulator for i in count(): - robot.updateJointSlider() + robot.update_joint_slider() if i % 60 == 0: - img = robot.left_camera.getRGBImage() + img = robot.left_camera.get_rgb_image() world.step(sleep_dt=1./240) diff --git a/examples/robots/wam.py b/examples/robots/wam.py index e6f562a..d9787c4 100644 --- a/examples/robots/wam.py +++ b/examples/robots/wam.py @@ -18,29 +18,29 @@ world = BasicWorld(sim) robot = WAM(sim) # print information about the robot -robot.printRobotInfo() -# H = robot.calculateMassMatrix() +robot.print_info() +# H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) -robot.setJointPositions([np.pi / 4, np.pi / 2], jointId=[0,1]) #2, 4]) +robot.set_joint_positions([np.pi / 4, np.pi / 2], joint_ids=[0, 1]) #2, 4]) -Jlin = robot.calculateJacobian(6, localPosition=(0., 0., 0.))[:3] -robot.drawVelocityManipulabilityEllipsoid(6, Jlin, color=(1,0,0,0.7)) +Jlin = robot.get_jacobian(6)[:3] +robot.draw_velocity_manipulability_ellipsoid(6, Jlin, color=(1, 0, 0, 0.7)) for _ in range(5): world.step(sleep_dt=1./240) -Jlin = robot.calculateJacobian(6, localPosition=(0., 0., 0.))[:3] -robot.drawVelocityManipulabilityEllipsoid(6, Jlin, color=(0, 0, 1, 0.7)) +Jlin = robot.get_jacobian(6)[:3] +robot.draw_velocity_manipulability_ellipsoid(6, Jlin, color=(0, 0, 1, 0.7)) for _ in range(45): world.step(sleep_dt=1./240) -Jlin = robot.calculateJacobian(6, localPosition=(0., 0., 0.))[:3] -robot.drawVelocityManipulabilityEllipsoid(6, Jlin) +Jlin = robot.get_jacobian(6)[:3] +robot.draw_velocity_manipulability_ellipsoid(6, Jlin) for i in count(): if i%1000 == 0: - print("Joint Torques: {}".format(robot.getJointTorques())) - print("Gravity Torques: {}".format(robot.getGravityCompensationTorques())) - print("Compensation Torques: {}".format(robot.getCoriolisAndGravityCompensationTorques())) + print("Joint Torques: {}".format(robot.get_joint_torques())) + print("Gravity Torques: {}".format(robot.get_gravity_compensation_torques())) + print("Compensation Torques: {}".format(robot.get_coriolis_and_gravity_compensation_torques())) # step in simulation world.step(sleep_dt=1./240) diff --git a/examples/robots/youbot.py b/examples/robots/youbot.py index 214a48f..e8824de 100644 --- a/examples/robots/youbot.py +++ b/examples/robots/youbot.py @@ -16,23 +16,23 @@ sim = BulletSim() world = BasicWorld(sim) # create robot -youbot_base = YoubotBase(sim, init_pos=(0, -0.75)) -kuka_arm = KukaYoubotArm(sim, init_pos=(0, -0.25)) -youbot = Youbot(sim, init_pos=(0, 0.25)) -youbot_dual_arm = YoubotDualArm(sim, init_pos=(0, 0.75)) +youbot_base = YoubotBase(sim, position=(0, -0.75)) +kuka_arm = KukaYoubotArm(sim, position=(0, -0.25)) +youbot = Youbot(sim, position=(0, 0.25)) +youbot_dual_arm = YoubotDualArm(sim, position=(0, 0.75)) robots = [youbot_base, kuka_arm, youbot, youbot_dual_arm] # print information about the robot for robot in robots: - robot.printRobotInfo() + robot.print_info() # Position control using sliders -# robot.addJointSlider() +# robot.add_joint_slider() # run simulator for _ in count(): - # robots[0].updateJointSlider() + # robots[0].update_joint_slider() # for robot in robots: # robot.drive(5) world.step(sleep_dt=1./240) diff --git a/pyrobolearn/actions/robot_actions/joint_actions.py b/pyrobolearn/actions/robot_actions/joint_actions.py index 7d7c683..0478e6d 100644 --- a/pyrobolearn/actions/robot_actions/joint_actions.py +++ b/pyrobolearn/actions/robot_actions/joint_actions.py @@ -37,7 +37,7 @@ class JointAction(RobotAction): # get the joints of the robot if joint_ids is None: - joint_ids = robot.getJointIds() + joint_ids = robot.get_joint_ids() elif isinstance(joint_ids, int): joint_ids = [joint_ids] self.joints = joint_ids @@ -47,7 +47,7 @@ class JointAction(RobotAction): # return len(self.joints) def bounds(self): - return self.robot.getJointLimits(self.joints) + return self.robot.get_joint_limits(self.joints) class JointPositionAction(JointAction): @@ -59,13 +59,13 @@ class JointPositionAction(JointAction): def __init__(self, robot, joint_ids=None, kp=None, kd=None, max_force=None): self.kp, self.kd, self.max_force = kp, kd, max_force super(JointPositionAction, self).__init__(robot, joint_ids) - self.data = robot.getJointPositions(self.joints) + self.data = robot.get_joint_positions(self.joints) def _write(self, data=None): if data is None: - self.robot.setJointPositions(self._data, self.joints, kp=self.kp, kd=self.kd, maxTorque=self.max_force) + self.robot.set_joint_positions(self._data, self.joints, kp=self.kp, kd=self.kd, forces=self.max_force) else: - self.robot.setJointPositions(data, self.joints, kp=self.kp, kd=self.kd, maxTorque=self.max_force) + self.robot.set_joint_positions(data, self.joints, kp=self.kp, kd=self.kd, forces=self.max_force) class JointVelocityAction(JointAction): @@ -76,13 +76,13 @@ class JointVelocityAction(JointAction): def __init__(self, robot, joint_ids=None): super(JointVelocityAction, self).__init__(robot, joint_ids) - self.data = robot.getJointVelocities(self.joints) + self.data = robot.get_joint_velocities(self.joints) def _write(self, data=None): if data is None: - self.robot.setJointVelocities(self._data, self.joints) + self.robot.set_joint_velocities(self._data, self.joints) else: - self.robot.setJointVelocities(data, self.joints) + self.robot.set_joint_velocities(data, self.joints) class JointForceAction(JointAction): @@ -93,16 +93,16 @@ class JointForceAction(JointAction): def __init__(self, robot, joint_ids=None, f_min=-np.infty, f_max=np.infty): super(JointForceAction, self).__init__(robot, joint_ids) - self.data = robot.getJointTorques(self.joints) + self.data = robot.get_joint_torques(self.joints) self.f_min = f_min self.f_max = f_max def _write(self, data=None): if data is None: - self.robot.setJointTorques(self._data, self.joints) + self.robot.set_joint_torques(self._data, self.joints) else: data = np.clip(data, self.f_min, self.f_max) - self.robot.setJointTorques(data, self.joints) + self.robot.set_joint_torques(data, self.joints) class JointAccelerationAction(JointAction): @@ -115,13 +115,13 @@ class JointAccelerationAction(JointAction): def __init__(self, robot, joint_ids=None, a_min=-np.infty, a_max=np.infty): super(JointAccelerationAction, self).__init__(robot, joint_ids) - self.data = robot.getJointAccelerations(self.joints) + self.data = robot.get_joint_accelerations(self.joints) self.a_min = a_min self.a_max = a_max def _write(self, data=None): if data is None: - self.robot.setJointAccelerations(self._data, self.joints) + self.robot.set_joint_accelerations(self._data, self.joints) else: data = np.clip(data, self.a_min, self.a_max) - self.robot.setJointAccelerations(data, self.joints) + self.robot.set_joint_accelerations(data, self.joints) diff --git a/pyrobolearn/actions/robot_actions/link_actions.py b/pyrobolearn/actions/robot_actions/link_actions.py index 0a96468..e786b4f 100644 --- a/pyrobolearn/actions/robot_actions/link_actions.py +++ b/pyrobolearn/actions/robot_actions/link_actions.py @@ -36,7 +36,7 @@ class LinkAction(RobotAction): # get the joints of the robot if link_ids is None: - link_ids = robot.getLinkIds() + link_ids = robot.get_link_ids() self.links = link_ids @@ -50,9 +50,9 @@ class LinkPositionAction(LinkAction): def _write(self, data=None): if data is None: - self.robot.setLinkPositions(self.links, self._data) + self.robot.set_link_positions(self.links, self._data) else: - self.robot.setLinkPositions(self.links, data) + self.robot.set_link_positions(self.links, data) ######################## @@ -63,7 +63,7 @@ class EndEffectorAction(LinkAction): def __init__(self, robot, end_effector_ids=None): if end_effector_ids is None: - end_effector_ids = robot.getEndEffectorIds() + end_effector_ids = robot.get_end_effector_ids() super(EndEffectorAction, self).__init__(robot, end_effector_ids) diff --git a/pyrobolearn/actions/robot_actions/robot_actions.py b/pyrobolearn/actions/robot_actions/robot_actions.py index baf44e7..2ff6962 100644 --- a/pyrobolearn/actions/robot_actions/robot_actions.py +++ b/pyrobolearn/actions/robot_actions/robot_actions.py @@ -10,8 +10,8 @@ Dependencies: from abc import ABCMeta -from pyrobolearn.actions.action import Action -from pyrobolearn.robots.robot import Robot +from pyrobolearn.actions import Action +from pyrobolearn.robots import Robot __author__ = "Brian Delhaisse" @@ -42,8 +42,8 @@ class RobotAction(Action): def robot(self): return self._robot - def isDiscrete(self): + def is_discrete(self): return False - def isContinuous(self): + def is_continuous(self): return True diff --git a/pyrobolearn/rewards/__init__.py b/pyrobolearn/rewards/__init__.py index 8f0b5cf..bd6f0c1 100644 --- a/pyrobolearn/rewards/__init__.py +++ b/pyrobolearn/rewards/__init__.py @@ -1,3 +1,13 @@ -# import rewards +# import abstract reward class and operations from .reward import * + +# import basic rewards +from .basic_rewards import * + +# import gym wrapper reward +from .gym_reward import GymReward + +# import costs +from .cost import * + diff --git a/pyrobolearn/rewards/basic_rewards.py b/pyrobolearn/rewards/basic_rewards.py new file mode 100644 index 0000000..f84b32e --- /dev/null +++ b/pyrobolearn/rewards/basic_rewards.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python +"""Define some basic rewards used in reinforcement learning and optimization. + +Dependencies: +- `pyrobolearn.states` +- `pyrobolearn.actions` +""" + +import numpy as np + +# from objective import Objective +from pyrobolearn.robots.robot import Robot +import pyrobolearn.states as states +import pyrobolearn.actions as actions +from pyrobolearn.rewards.reward import Reward + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "MIT" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class FixedReward(Reward): + r"""Fixed reward. + + This is a dummy class which always returns a fixed reward. This is fixed initially. + """ + + def __init__(self, value): + super(FixedReward, self).__init__() + if not isinstance(value, (int, float)): + raise TypeError("Expecting a number") + self.value = value + + def __repr__(self): + return '%s(%s)' % (self.__class__.__name__, str(self.value)) + + def compute(self): + return self.value + + +class FunctionalReward(Reward): + r"""Functional reward. + + This is a reward class which calls a given function/class to compute the reward. + """ + def __init__(self, function): + super(FunctionalReward, self).__init__() + self.function = function + + def __repr__(self): + return self.function.__name__ + + def compute(self): + return self.function() + + +class ForwardProgressReward(Reward): + r"""Forward progress reward + + Compute the forward progress based on a forward direction, a previous and current positions. + """ + + def __init__(self, state, direction=(1, 0, 0), normalize=False, update_state=False): + """ + Initialize the Forward Progress Reward. + + Args: + state (BasePositionState, PositionState, Robot): robot or base position state. + direction (np.float[3], None): forward direction vector. If None, it will take the initial forward vector. + normalize (bool): if we should normalize the direction vector. + update_state (bool): if we should call the state and update its value. + """ + # check state argument + self.update_state = update_state + if isinstance(state, Robot): + state = states.BasePositionState(state) + self.update_state = True + elif not isinstance(state, (states.BasePositionState, states.PositionState)): + raise TypeError("Expecting the state to be an instance of `BasePositionState`, `PositionState`, or `Robot`" + ", instead got: {}".format(type(state))) + super(ForwardProgressReward, self).__init__(state=state) + + # if no direction specified, take the body forward vector + if direction is None: + self.direction = state.body.forward_vector + else: + self.direction = np.array(direction) + + # normalize the direction vector if specified + if normalize: + self.direction = self.normalize(self.direction) + + # remember current position + self.prev_pos = np.copy(self.state.data[0]) + self.value = 0 + + @staticmethod + def normalize(x): + """ + Normalize the given vector. + """ + if np.allclose(x, 0): + return x + return x / np.linalg.norm(x) + + def compute(self): + """Compute the difference vector between the current and previous position (i.e. ~ velocity vector), and + compute the dot product between this velocity vector and the direction vector.""" + if self.update_state: + self.state() + curr_pos = self.state.data[0] + velocity = curr_pos - self.prev_pos + self.value = self.direction.dot(velocity) + self.prev_pos = np.copy(curr_pos) + return self.value + + +class DirectiveReward(Reward): + r"""Directive Reward + + Provide reward if the vector state is in the specified direction. Specifically, it computes the dot product + between the state vector and the specified direction. + + If normalize, the reward is between -1 and 1. + """ + + def __init__(self, state, direction=(1, 0, 0), normalize=True): + super(DirectiveReward, self).__init__(state=state) + + self.normalize = normalize + if self.normalize: + self.direction = self.norm(np.array(direction)) + + # TODO uncomment + # if not isinstance(state, (PositionState, BasePositionState)): + # raise ValueError("Expecting state to be a PositionState or BasePositionState") + self.value = 0 + + @staticmethod + def norm(x): + """ + Normalize the given vector. + """ + if np.allclose(x, 0): + return x + return x / np.linalg.norm(x) + + def compute(self): + pos = self.state.data[0] + if self.normalize: + pos = self.norm(pos) + self.value = self.direction.dot(pos) + return self.value + + +class L2SimilarityReward(Reward): + """ + Compute the square of the L2 norm between two vectors. + """ + + def __init__(self): + super(L2SimilarityReward, self).__init__() + + def value(self, vector1, vector2): + return np.dot(vector1, vector2) + + +class ImitationReward(Reward): + + def __init__(self, human, robot): + super(ImitationReward, self).__init__() + self.human = human # instance of HumanKinematic class + self.robot = robot # instance of Robot class + + def compute(self): + # check + pass diff --git a/pyrobolearn/rewards/cost.py b/pyrobolearn/rewards/cost.py index 2534183..736a076 100644 --- a/pyrobolearn/rewards/cost.py +++ b/pyrobolearn/rewards/cost.py @@ -19,7 +19,10 @@ import copy import numpy as np import torch +from pyrobolearn.robots.robot import Robot # from objective import Objective +import pyrobolearn.states as states +import pyrobolearn.actions as actions from pyrobolearn.rewards.reward import Reward @@ -182,7 +185,7 @@ class TorqueCost(Cost): self.tau = torque_state def compute(self): - return - np.sum(self.tau.data**2) + return - np.sum(self.tau.data[0]**2) class PowerCost(Cost): @@ -199,7 +202,51 @@ class PowerCost(Cost): self.vel = velocity_state def compute(self): - return - np.sum(np.maximum(self.tau.data * self.vel.data, 0)) + return - np.sum(np.maximum(self.tau.data[0] * self.vel.data[0], 0)) + + +class JointPowerConsumptionCost(Cost): + r"""Joint Power Consumption Cost + + Return the joint power consumption cost, where the power is computed as the torque times the velocity. + """ + + def __init__(self, state, joint_ids=None, update_state=False): + """ + Initialize the Joint Power Consumption cost. + + Args: + torque (Robot, State): robot instance, or the state. The state must contains the `JointForceTorqueState` + and the `JointVelocityState`. + joint_ids (None, int, list of int): joint ids. This used if `torque` is a `Robot` instance. + update_state (bool): If True, it will update the state. + """ + self.update_state = update_state + if isinstance(state, Robot): + torque = states.JointForceTorqueState(state, joint_ids=joint_ids) + velocity = states.JointVelocityState(state, joint_ids=joint_ids) + self.update_state = True + # state = torque + velocity + elif isinstance(state, states.State): + # check if they have the correct state + torque = state.lookfor(states.JointForceTorqueState) + if torque is None: + raise ValueError("Didn't find a `JointForceTorqueState` instance in the given states.") + velocity = state.lookfor(states.JointVelocityState) + if velocity is None: + raise ValueError("Didn't find a `JointVelocityState` instance in the given states.") + else: + raise TypeError("Expecting the state to be an instance of `State` or `Robot`.") + super(JointPowerConsumptionCost, self).__init__() # state=state) + self.tau = torque + self.vel = velocity + self.update_state = update_state + + def compute(self): + if self.update_state: + self.tau() + self.vel() + return - np.sum((self.tau.data[0] * self.vel.data[0])**2) class JointAccelerationCost(Cost): @@ -369,7 +416,7 @@ class PowerConsumptionCost(Cost): super(PowerConsumptionCost, self).__init__() def loss(self, robot): - return np.dot(robot.getJointTorques(), robot.getJointVelocities()) + return np.dot(robot.get_joint_torques(), robot.get_joint_velocities()) class DistanceCost(Cost): diff --git a/pyrobolearn/rewards/gym_reward.py b/pyrobolearn/rewards/gym_reward.py new file mode 100644 index 0000000..71bdfba --- /dev/null +++ b/pyrobolearn/rewards/gym_reward.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python +"""Provide the Gym wrapper reward. +""" + +from pyrobolearn.rewards.basic_rewards import FixedReward + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "MIT" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class GymReward(FixedReward): + r"""OpenAI Gym reward + + This provides a wrapper around the gym reward value. It is the same as the FixedReward. + """ + pass diff --git a/pyrobolearn/rewards/reward.py b/pyrobolearn/rewards/reward.py index dee0dbe..822b161 100644 --- a/pyrobolearn/rewards/reward.py +++ b/pyrobolearn/rewards/reward.py @@ -216,6 +216,10 @@ class Reward(object): def is_maximized(): return True + def reset(self): + for reward in self.rewards: + reward.reset() + def compute(self): pass @@ -645,169 +649,7 @@ def trunc(x): # Rewards # ############################################################## -class FixedReward(Reward): - r"""Fixed reward. - This is a dummy class which always returns a fixed reward. This is fixed initially. - """ - - def __init__(self, value): - super(FixedReward, self).__init__() - if not isinstance(value, (int, float)): - raise TypeError("Expecting a number") - self.value = value - - def __repr__(self): - return '%s(%s)' % (self.__class__.__name__, str(self.value)) - - def compute(self): - return self.value - - -class FunctionalReward(Reward): - r"""Functional reward. - - This is a reward class which calls a given function/class to compute the reward. - """ - def __init__(self, function): - super(FunctionalReward, self).__init__() - self.function = function - - def __repr__(self): - return self.function.__name__ - - def compute(self): - return self.function() - - -class ForwardProgressReward(Reward): - r"""Forward progress reward - - Compute the forward progress based on a forward direction, a previous and current positions. - """ - - def __init__(self, state, direction=(1, 0, 0), normalize=False): - super(ForwardProgressReward, self).__init__(state=state) - - # if direction is None: - # # takes the robot initial direction - # #direction = ... - # #init_pos - # pass - # if isinstance(direction, np.ndarray): - # pass - # elif isinstance(direction, Robot): - # # takes the - # pass - # - # self.direction = direction - # #self.init_pos = init_pos - - self.direction = self.normalize(np.array(direction)) - - # TODO uncomment - # if not isinstance(state, (PositionState, BasePositionState)): - # raise ValueError("Expecting state to be a PositionState or BasePositionState") - self.init_pos = np.copy(self.state._data) - self.value = 0 - - @staticmethod - def normalize(x): - """ - Normalize the given vector. - """ - if np.allclose(x, 0): - return x - return x / np.linalg.norm(x) - - def compute(self): - curr_pos = self.state._data - delta_pos = curr_pos - self.init_pos - self.value = self.direction.dot(delta_pos) - # self.value = curr_pos[0] - self.init_pos[0] - self.init_pos = np.copy(curr_pos) - return self.value - - -class DirectiveReward(Reward): - r"""Directive Reward - - Provide reward if the vector state is in the specified direction. Specifically, it computes the dot product - between the state vector and the specified direction. - - If normalize, the reward is between -1 and 1. - """ - - def __init__(self, state, direction=(1, 0, 0), normalize=True): - super(DirectiveReward, self).__init__(state=state) - - self.normalize = normalize - if self.normalize: - self.direction = self.norm(np.array(direction)) - - # TODO uncomment - # if not isinstance(state, (PositionState, BasePositionState)): - # raise ValueError("Expecting state to be a PositionState or BasePositionState") - self.value = 0 - - @staticmethod - def norm(x): - """ - Normalize the given vector. - """ - if np.allclose(x, 0): - return x - return x / np.linalg.norm(x) - - def compute(self): - pos = self.state._data - if self.normalize: - pos = self.norm(pos) - self.value = self.direction.dot(pos) - return self.value - - -class L2SimilarityReward(Reward): - """ - Compute the square of the L2 norm between two vectors. - """ - - def __init__(self): - super(L2SimilarityReward, self).__init__() - - def value(self, vector1, vector2): - return np.dot(vector1, vector2) - - -class ImitationReward(Reward): - - def __init__(self, human, robot): - super(ImitationReward, self).__init__() - self.human = human # instance of HumanKinematic class - self.robot = robot # instance of Robot class - - def compute(self): - # check - pass - - -class GymReward(Reward): - r"""OpenAI Gym reward - - This provides a wrapper - """ - - def __init__(self, value): - super(GymReward, self).__init__() - if not isinstance(value, (int, float)): - raise TypeError("Expecting a number") - self.value = value - - def __repr__(self): - return '%s(%s)' % (self.__class__.__name__, str(self.value)) - - def compute(self): - return self.value # Test diff --git a/pyrobolearn/rewards/reward_processor.py b/pyrobolearn/rewards/reward_processor.py new file mode 100644 index 0000000..54b709a --- /dev/null +++ b/pyrobolearn/rewards/reward_processor.py @@ -0,0 +1,284 @@ +#!/usr/bin/env python +"""Provide some reward processors. + +It processes the rewards before returning them; this can be useful to standardize, normalize, center them for instance. +""" + +import numpy as np +from reward import Reward + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "MIT" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RewardProcessor(Reward): + r"""Reward Processor + + Wraps the reward and process it. It also acts as a memory of the last received reward signal. + + Examples: + reward = Reward1() + Reward2() + reward = RewardProcessor(reward, ) + """ + + def __init__(self, reward): + super(RewardProcessor, self).__init__() + self.reward = reward + self.value = 0 + + def compute(self): + self.value = self.reward() + return self.value + + +class ShiftRewardProcessor(RewardProcessor): + r"""Shift Reward Processor + + Shift the reward by the given amount; that is, it returned: :math:`\hat{r} = r + x` where :math:`x` is the + specified amount to shift the original reward. + """ + + def __init__(self, reward, x): + """ + Initialize the Center Reward Processor. + + Args: + reward (Reward): Reward instance to process. + x (int, float): amount to be shifted. + """ + super(ShiftRewardProcessor, self).__init__(reward) + self.x = x + + def compute(self): + reward = self.reward() + self.value = reward + self.x + return self.value + + +class ClipRewardProcessor(RewardProcessor): + r"""Clip Reward Processor + + Processor that clips the given reward to be between [low, high], where `low` and `high` are respectively the + specified lower and higher bound. + """ + + def __init__(self, reward, low=-10, high=10): + """ + Initialize the Clip processor. + + Args: + reward (Reward): Reward instance to process. + low (int, float): lower bound + high (int, float): higher bound + """ + super(ClipRewardProcessor, self).__init__(reward) + self.low = low + self.high = high + + def compute(self): + reward = self.reward() + self.value = np.clip(reward, self.low, self.high) + return self.value + + +class CenterRewardProcessor(RewardProcessor): + r"""Center Reward Processor + + Center the reward using the running mean. + """ + + def __init__(self, reward): + super(CenterRewardProcessor, self).__init__(reward) + self.mean = 0 + self.N = 0 + + def reset(self): + self.mean = 0 + self.N = 0 + self.reward.reset() + + def compute(self): + reward = self.reward() + # update the mean + self.mean = self.N / (self.N + 1.) * self.mean + 1. / (self.N + 1) * reward + self.N += 1 + # center reward + self.value = self.value - self.mean + return self.value + + +class NormalizeRewardProcessor(RewardProcessor): + r"""Normalize Reward Processor + + Normalize the reward such that it is between 0 and 1. That is, it returned + :math:`\hat{r} = \frac{r - r_{min}}{r_{max} - r_{min}}`, where :math:`r \in [r_{min}, r_{max}]`. + + Warnings: the first returned reward will be 0. + """ + + def __init__(self, reward): + super(NormalizeRewardProcessor, self).__init__(reward) + self.min = np.infty + self.max = -np.infty + + def reset(self): + self.min = np.infty + self.max = -np.infty + self.reward.reset() + + def compute(self): + reward = self.reward() + self.min = np.minimum(reward, self.min) + self.max = np.maximum(reward, self.max) + den = self.max - self.min + if den == 0: + den = 1. + self.value = (reward - self.min) / den + return self.value + + +class StandardizeRewardProcessor(RewardProcessor): + r"""Standardize Reward Processor + + Standardize the reward such that it returns :math:`\hat{r} = \frac{r - \mu}{\sigma}` where :math:`\mu` is the + running mean, and :math:`\sigma` is the running standard deviation. The returned reward will have a mean of 0 + and standard deviation of 1. + """ + + def __init__(self, reward, epsilon=1.e-4, center=True): + super(StandardizeRewardProcessor, self).__init__(reward) + self.eps = epsilon + self.mean = 0 + self.var = 1 + self.N = 0 + self.center = center + + def reset(self): + self.mean = 0 + self.var = 1 + self.N = 0 + self.reward.reset() + + def compute(self): + reward = self.reward() + + # update the mean + old_mean = self.mean + self.mean = self.N / (self.N + 1.) * self.mean + 1. / (self.N + 1) * reward + + # update the var / stddev + self.var = self.N / (self.N + 1) * self.var + \ + 1. / (self.N + 1) * (self.value - old_mean) * (self.value - self.mean) + std = np.sqrt(self.var) + + # update total number of data points + self.N += 1 + + # standardize the reward + if self.center: + self.value = (reward - self.mean) / (std + self.eps) + else: + self.value = reward / (std + self.eps) + return self.value + + +class GammaAccumulatedRewardProcessor(RewardProcessor): + r"""Gamma reward processor + + It will return the accumulated reward until now: :math:`R = \sum_{t'=0}^t \gamma^{t'} r_{t'}`. + """ + + def __init__(self, reward, gamma=0.99): + super(GammaAccumulatedRewardProcessor, self).__init__(reward) + self.gamma = gamma + self.ret = 0. + + def reset(self): + self.ret = 0. + self.reward.reset() + + def compute(self): + reward = self.reward() + self.ret = reward + self.gamma * self.ret + return self.ret + + +class GammaStandardizeRewardProcessor(RewardProcessor): + r"""Gamma Standardize Reward Processor + + References: + [1] https://github.com/openai/baselines/blob/master/baselines/common/vec_env/vec_normalize.py + """ + + def __init__(self, reward, gamma=0.99, epsilon=1.e-4): + super(GammaStandardizeRewardProcessor, self).__init__(reward) + self.gamma = gamma + self.eps = epsilon + self.ret = 0 + self.mean = 0 + self.var = 1 + self.N = 0 + + def reset(self): + self.ret = 0 + self.mean = 0 + self.var = 1 + self.N = 0 + self.reward.reset() + + def compute(self): + reward = self.reward() + + # update return + self.ret = reward + self.gamma * self.ret + + # update the return mean + old_mean = self.mean + self.mean = self.N / (self.N + 1.) * self.mean + 1. / (self.N + 1) * self.ret + + # update the return variance + self.var = self.N / (self.N + 1) * self.var + 1. / (self.N + 1) * (self.ret - old_mean) * (self.ret - self.mean) + std = np.sqrt(self.var) + + # update total number of data points + self.N += 1 + + self.value = reward / (std + self.eps) + return self.value + + +class ScaleRewardProcessor(RewardProcessor): + r"""Scale Reward Processor + + Processor that scales the reward x which is between [x1, x2] to the output y which is between [y1, y2]. + """ + + def __init__(self, reward, x1, x2, y1, y2): + """ + Initialize the scale reward processor + + Args: + reward (Reward): reward function to process. + x1 (int, float): lower bound of the original reward + x2 (int, float): upper bound of the original reward + y1 (int, float): lower bound of the final reward + y2 (int, float): upper bound of the final reward + """ + super(ScaleRewardProcessor, self).__init__(reward) + self.x1 = x1 + self.x2 = x2 + self.y1 = y1 + self.y2 = y2 + self.ratio = (self.y2 - self.y1) / (self.x2 - self.x1) + + @convert_numpy + def compute(self): + reward = self.reward() + self.value = self.y1 + (reward - self.x1) * self.ratio + return self.value diff --git a/pyrobolearn/robots/aibo.py b/pyrobolearn/robots/aibo.py index 1158320..f99f046 100644 --- a/pyrobolearn/robots/aibo.py +++ b/pyrobolearn/robots/aibo.py @@ -17,31 +17,31 @@ class Aibo(QuadrupedRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.02), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.02), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/aibo/aibo.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/aibo/aibo.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.02) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.02,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.02) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.02,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Aibo, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Aibo, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'aibo' - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['LF_up_leg_rot_y', 'LF_up_leg_rot_x', 'LF_down_leg'], ['RF_up_leg_rot_y', 'RF_up_leg_rot_x', 'RF_down_leg'], ['LB_up_leg_rot_y', 'LB_up_leg_rot_x', 'LB_down_leg'], ['RB_up_leg_rot_y', 'RB_up_leg_rot_x', 'RB_down_leg']]] - self.feet = [self.getLinkIds(link) for link in ['LF_paw', 'RF_paw', 'LB_paw', 'RB_paw'] + self.feet = [self.get_link_ids(link) for link in ['LF_paw', 'RF_paw', 'LB_paw', 'RB_paw'] if link in self.link_names] @@ -61,12 +61,12 @@ if __name__ == "__main__": robot = Aibo(sim) # , useFixedBase=True) # print information about the robot - robot.printRobotInfo() + robot.print_info() # # Position control using sliders - # robot.addJointSlider(robot.getLeftFrontLegIds() + robot.getRightFrontLegIds()) + # robot.add_joint_slider(robot.getLeftFrontLegIds() + robot.getRightFrontLegIds()) # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/allegrohand.py b/pyrobolearn/robots/allegrohand.py index 7b8a186..b221d8b 100644 --- a/pyrobolearn/robots/allegrohand.py +++ b/pyrobolearn/robots/allegrohand.py @@ -18,27 +18,27 @@ class AllegroHand(Hand): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), + position=(0, 0, 0), + orientation=(0, 0, 0, 1), scaling=1., left=False, - useFixedBase=True): + fixed_base=True): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True # if left: # urdf_path = '../robots/urdfs/allegrohand/allegro_left_hand.urdf' # else: urdf_path = os.path.dirname(__file__) + '/urdfs/allegrohand/allegro_right_hand.urdf' - super(AllegroHand, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(AllegroHand, self).__init__(simulator, urdf_path, position, orientation, fixed_base, scaling) self.name = 'allegro_hand' @@ -58,16 +58,16 @@ if __name__ == "__main__": right_hand = AllegroHand(sim) # , init_pos=(0.,0.,0.), init_orient=(0,0,1,0)) # print information about the robot - right_hand.printRobotInfo() - # H = right_hand.calculateMassMatrix() + right_hand.print_info() + # H = right_hand.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) # Position control using sliders - right_hand.addJointSlider() + right_hand.add_joint_slider() for i in count(): - right_hand.updateJointSlider() - # right_hand.setJointPositions([0.] * right_hand.getNumberOfDoFs()) + right_hand.update_joint_slider() + # right_hand.set_joint_positions([0.] * right_hand.getNumberOfDoFs()) # step in simulation world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/ant.py b/pyrobolearn/robots/ant.py index c8005a8..fb6ce13 100644 --- a/pyrobolearn/robots/ant.py +++ b/pyrobolearn/robots/ant.py @@ -13,31 +13,31 @@ class Ant(QuadrupedRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.2), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.2), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/mjcfs/ant.xml'): + urdf=os.path.dirname(__file__) + '/mjcfs/ant.xml'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.2) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.2,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.2) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.2,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Ant, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Ant, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'ant' - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['link0_3', 'link0_5'], ['link0_18', 'link0_20'], ['link0_13', 'link0_15'], ['link0_8', 'link0_10']]] - self.feet = [self.getLinkIds(link) for link in ['front_left_foot', 'front_right_foot', + self.feet = [self.get_link_ids(link) for link in ['front_left_foot', 'front_right_foot', 'left_back_foot', 'right_back_foot'] if link in self.link_names] @@ -58,12 +58,12 @@ if __name__ == "__main__": robot = Ant(sim) # , useFixedBase=True) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider(robot.getLeftFrontLegIds() + robot.getRightFrontLegIds()) + # robot.add_joint_slider(robot.getLeftFrontLegIds() + robot.getRightFrontLegIds()) # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/atlas.py b/pyrobolearn/robots/atlas.py index f952a41..e8e7a61 100644 --- a/pyrobolearn/robots/atlas.py +++ b/pyrobolearn/robots/atlas.py @@ -20,39 +20,39 @@ class Atlas(BipedRobot, BiManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 1.), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 1.), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/atlas/atlas_v4_with_multisense.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/atlas/atlas_v4_with_multisense.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 1.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (1.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 1.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (1.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Atlas, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Atlas, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'atlas' - self.head = self.getLinkIds('head') if 'head' in self.link_names else None + self.head = self.get_link_ids('head') if 'head' in self.link_names else None - self.torso = [self.getLinkIds(link) for link in ['ltorso', 'mtorso', 'utorso'] if link in self.link_names] + self.torso = [self.get_link_ids(link) for link in ['ltorso', 'mtorso', 'utorso'] if link in self.link_names] - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['l_uglut', 'l_lglut', 'l_uleg', 'l_lleg', 'l_talus', 'l_foot'], ['r_uglut', 'r_lglut', 'r_uleg', 'r_lleg', 'r_talus', 'r_foot']]] - self.feet = [self.getLinkIds(link) for link in ['l_foot', 'r_foot'] if link in self.link_names] + self.feet = [self.get_link_ids(link) for link in ['l_foot', 'r_foot'] if link in self.link_names] - self.arms = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.arms = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['l_clav', 'l_scap', 'l_uarm', 'l_larm', 'l_ufarm', 'l_lfarm', 'l_hand'], ['r_clav', 'r_scap', 'r_uarm', 'r_larm', 'r_ufarm', 'r_lfarm', 'r_hand']]] - self.hands = [self.getLinkIds(link) for link in ['l_hand', 'r_hand'] if link in self.link_names] + self.hands = [self.get_link_ids(link) for link in ['l_hand', 'r_hand'] if link in self.link_names] # Test @@ -71,12 +71,12 @@ if __name__ == "__main__": robot = Atlas(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # position control using sliders - robot.addJointSlider(robot.getLeftLegIds()) + robot.add_joint_slider(robot.getLeftLegIds()) # run simulator for _ in count(): - robot.updateJointSlider() + robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/ballbot.py b/pyrobolearn/robots/ballbot.py index 495c1c9..956edbc 100644 --- a/pyrobolearn/robots/ballbot.py +++ b/pyrobolearn/robots/ballbot.py @@ -16,20 +16,20 @@ class Ballbot(Robot): [2] http://www.theconstructsim.com/bb-8-gazebo-model/ """ - def __init__(self, simulator, init_pos=(0, 0, 0.), init_orient=(0, 0, 0, 1), useFixedBase=False, - scaling=1., urdf_path=os.path.dirname(__file__) + '/urdfs/ballbot/ballbot.urdf'): + def __init__(self, simulator, position=(0, 0, 0.), orientation=(0, 0, 0, 1), fixed_base=False, + scaling=1., urdf=os.path.dirname(__file__) + '/urdfs/ballbot/ballbot.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Ballbot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) - self.ball = self.sim.loadURDF(os.path.dirname(__file__) + '/urdfs/ballbot/ball.urdf', init_pos, init_orient) + super(Ballbot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) + self.ball = self.sim.load_urdf(os.path.dirname(__file__) + '/urdfs/ballbot/ball.urdf', position, orientation) self.name = 'ballbot' @@ -49,10 +49,10 @@ if __name__ == "__main__": robot = Ballbot(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() for i in count(): - # robot.setJointVelocities([0, -1, 0]) + # robot.set_joint_velocities([0, -1, 0]) # step in simulation world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/base.py b/pyrobolearn/robots/base.py index 9c41a0b..67c0b6c 100644 --- a/pyrobolearn/robots/base.py +++ b/pyrobolearn/robots/base.py @@ -29,7 +29,7 @@ class Body(object): Define a physical body in the simulator/world. """ - def __init__(self, simulator, body_id, name=None): + def __init__(self, simulator, body_id=0, name=None): """ Initialize the Body. @@ -41,6 +41,7 @@ class Body(object): self.simulator = simulator self.id = body_id self.name = name + self._mass = None self.joints = None ############## @@ -68,8 +69,8 @@ class Body(object): @id.setter def id(self, id_): """Set the unique body id.""" - if not isinstance(id_, int): - raise TypeError("Expecting the given simulator to be an integer, instead got: {}".format(type(id_))) + # if not isinstance(id_, int): + # raise TypeError("Expecting the given simulator to be an integer, instead got: {}".format(type(id_))) self._id = id_ @property @@ -82,10 +83,32 @@ class Body(object): @name.setter def name(self, name): """Set the name of the body.""" + if name is None: + name = self.__class__.__name__.lower() if not isinstance(name, str): raise TypeError("Expecting the given name to be a string, instead got: {}".format(type(name))) self._name = name + @property + def base_link_id(self): + """Return the base link id.""" + return -1 + + @property + def base_name(self): + """Return the base name.""" + return self.sim.get_base_name(self.id) + + @property + def base_mass(self): + """Return the base mass.""" + return self.sim.get_base_mass(self.id) + + @property + def base_linear_momentum(self): + """Return the base linear momentum. Warnings: this is not the same as the total linear momentum.""" + return self.mass * self.linear_velocity + @property def pose(self): """Return the body pose.""" @@ -114,6 +137,21 @@ class Body(object): """Return the orientation as a rotation matrix.""" return get_matrix_from_quaternion(self.orientation) + @property + def forward_vector(self): + """Return the vector pointing forward.""" + return self.rotation_matrix[:, 0] + + @property + def left_vector(self): + """Return the vector pointing on the left.""" + return self.rotation_matrix[:, 1] + + @property + def up_vector(self): + """Return the vector pointing upward.""" + return self.rotation_matrix[:, 2] + @property def linear_velocity(self): """Return the linear velocity of the body's base.""" @@ -136,7 +174,9 @@ class Body(object): @property def mass(self): """Return the total mass of the body.""" - return self.sim.get_mass(self.id) + if self._mass is None: + self._mass = self.sim.get_mass(self.id) + return self._mass @property def dimensions(self): @@ -153,22 +193,10 @@ class Body(object): """Return the total number of links. This is the same as the number of joints.""" return self.sim.num_links(self.id) - @property - def num_actuated_joints(self): - """Return the total number of actuated joints. This property should be overwritten in the child class.""" - return self.sim.num_actuated_joints(self.id) - - @property - def actuated_joints(self): - """Return the total number of actuated joints.""" - if self.joints is None: - self.joints = self.sim.get_actuated_joint_ids(self.id) - return self.joints - @property def center_of_mass(self): """Return the center of mass.""" - return self.sim.get_center_of_mass(self.id) + return self.sim.get_center_of_mass_position(self.id) class MovableBody(Body): @@ -177,7 +205,7 @@ class MovableBody(Body): Define a movable object in the world. """ - def __init__(self, simulator, object_id, name=None): + def __init__(self, simulator, object_id=0, name=None): super(MovableBody, self).__init__(simulator, object_id, name=name) # def move(self, position=None, orientation=None): @@ -190,5 +218,19 @@ class ControllableBody(MovableBody): Define a controllable object in the world. """ - def __init__(self, simulator, object_id, name=None): + def __init__(self, simulator, object_id=0, name=None): super(ControllableBody, self).__init__(simulator, object_id, name=name) + + @property + def actuated_joints(self): + """Return the total number of actuated joints.""" + if self.joints is None: + self.joints = self.sim.get_actuated_joint_ids(self.id) + return self.joints + + @property + def num_actuated_joints(self): + """Return the total number of actuated joints. This property should be overwritten in the child class.""" + if self.joints is None: + return self.sim.num_actuated_joints(self.id) + return len(self.joints) diff --git a/pyrobolearn/robots/baxter.py b/pyrobolearn/robots/baxter.py index fc75505..fb32187 100644 --- a/pyrobolearn/robots/baxter.py +++ b/pyrobolearn/robots/baxter.py @@ -18,27 +18,27 @@ class Baxter(BiManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.95), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.95), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/baxter/baxter.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/baxter/baxter.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.95) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.95,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.95) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.95,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Baxter, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Baxter, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'baxter' - self.head = self.getLinkIds('head') if 'head' in self.link_names else None + self.head = self.get_link_ids('head') if 'head' in self.link_names else None - self.arms = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.arms = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow', 'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm', 'left_wrist', 'l_gripper_l_finger', 'l_gripper_r_finger'], @@ -46,7 +46,7 @@ class Baxter(BiManipulatorRobot): 'right_lower_elbow', 'right_upper_forearm', 'right_lower_forearm', 'right_wrist', 'r_gripper_l_finger', 'r_gripper_r_finger']]] - self.hands = [self.getLinkIds(link) for link in ['left_gripper', 'right_gripper'] if link in self.link_names] + self.hands = [self.get_link_ids(link) for link in ['left_gripper', 'right_gripper'] if link in self.link_names] if __name__ == "__main__": @@ -64,12 +64,12 @@ if __name__ == "__main__": robot = Baxter(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/bb8.py b/pyrobolearn/robots/bb8.py index 8f9623d..cab87d4 100644 --- a/pyrobolearn/robots/bb8.py +++ b/pyrobolearn/robots/bb8.py @@ -15,19 +15,19 @@ class BB8(Robot): [2] http://www.theconstructsim.com/bb-8-gazebo-model/ """ - def __init__(self, simulator, init_pos=(0, 0, 0.4), init_orient=(0, 0, 0, 1), useFixedBase=False, - scaling=1., urdf_path=os.path.dirname(__file__) + '/urdfs/bb8/bb8.urdf'): + def __init__(self, simulator, position=(0, 0, 0.4), orientation=(0, 0, 0, 1), fixed_base=False, + scaling=1., urdf=os.path.dirname(__file__) + '/urdfs/bb8/bb8.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.4) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.4,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.4) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.4,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(BB8, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(BB8, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'bb8' @@ -47,10 +47,10 @@ if __name__ == "__main__": robot = BB8(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() for i in count(): - robot.setJointVelocities([0, -1, 0]) + robot.set_joint_velocities([0, -1, 0]) # step in simulation world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/blackbird.py b/pyrobolearn/robots/blackbird.py index 8d88941..09598aa 100644 --- a/pyrobolearn/robots/blackbird.py +++ b/pyrobolearn/robots/blackbird.py @@ -20,44 +20,44 @@ class Blackbird(BipedRobot): def __init__(self, simulator, - init_pos=(0, 0, 1.2), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 1.2), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/blackbird/blackbird_biped.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/blackbird/blackbird_biped.urdf'): self.height = 1.2 self.base_height = self.height # check parameters - if init_pos is None: - init_pos = (0., 0., self.height) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (self.height,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., self.height) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (self.height,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Blackbird, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Blackbird, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'blackbird' # TODO: create constraints in pybullet # From the user guide: "URDF, SDF and MJCF specify articulated bodies as a tree-structures without loops. # The 'createConstraint' allows you to connect specific links of bodies to close those loops." - # self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + # self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] # for links in [['left_pelvis_rotation', 'left_hip', 'left_thigh', 'left_knee', 'left_shin', # 'left_tarsus', 'left_toe'], # ['right_pelvis_rotation', 'right_hip', 'right_thigh', 'right_knee', 'right_shin', # 'right_tarsus', 'right_toe']]] # - # self.feet = [self.getLinkIds(link) for link in ['left_toe', 'right_toe'] if link in self.link_names] + # self.feet = [self.get_link_ids(link) for link in ['left_toe', 'right_toe'] if link in self.link_names] # # # set joint angles to home position - # self.setJointHomePositions() + # self.set_joint_home_positions() - def getHomeJointPositions(self): + def get_home_joint_positions(self): """Return the joint positions for the home position""" return np.array([0, 0, 1.0204, -1.97, -0.084, 2.06, -1.9, 0, 0, 1.0204, -1.97, -0.084, 2.06, -1.9]) @@ -77,13 +77,13 @@ if __name__ == "__main__": robot = Blackbird(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - # robot.moveJointHomePositions() + # robot.update_joint_slider() + # robot.move_joint_home_positions() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/cartpole.py b/pyrobolearn/robots/cartpole.py index 5059b64..fbca817 100644 --- a/pyrobolearn/robots/cartpole.py +++ b/pyrobolearn/robots/cartpole.py @@ -9,7 +9,7 @@ import sympy import sympy.physics.mechanics as mechanics from pyrobolearn.robots.robot import Robot -from pyrobolearn.utils.orientation import get_symbolic_matrix_from_axis_angle +from pyrobolearn.utils.orientation import get_symbolic_matrix_from_axis_angle, get_matrix_from_quaternion class CartPole(Robot): @@ -28,25 +28,25 @@ class CartPole(Robot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), + position=(0, 0, 0), + orientation=(0, 0, 0, 1), scaling=1., - useFixedBase=True, - urdf_path=os.path.join(pybullet_data.getDataPath(), "cartpole.urdf"), + fixed_base=True, + urdf=os.path.join(pybullet_data.getDataPath(), "cartpole.urdf"), num_links=1, inverted_pole=False, pole_mass=1): # pole_mass=10 # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True - super(CartPole, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(CartPole, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'cartpole' # create dynamically other links if necessary @@ -55,7 +55,7 @@ class CartPole(Robot): # 2. https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_utils/urdfEditor.py # if num_links > 1: # # get info about the link (dimensions, mass, etc). - # jointInfo = self.getJointInfo(self.joints[-1]) + # jointInfo = self.get_joint_info(self.joints[-1]) # linkInfo = self.getLinkStates(self.joints[-1]) # dynamicInfo = self.sim.g # @@ -63,19 +63,19 @@ class CartPole(Robot): # # info = # jointType = info[2] - # jointAxis, parent = info[-4] + # joint_axis, parent = info[-4] # parentLinkIndex = info # # # create visual, collision shapes, and the body - # collision_shape = self.sim.createCollisionShape(self.sim.GEOM_BOX, halfExtents=dimensions) - # visual_shape = self.sim.createVisualShape(self.sim.GEOM_BOX, halfExtents=dimensions, rgbaColor=color) + # collision_shape = self.sim.create_collision_shape(self.sim.GEOM_BOX, half_extents=dimensions) + # visual_shape = self.sim.create_visual_shape(self.sim.GEOM_BOX, half_extents=dimensions, rgba_color=color) # # for i in range(num_links - 1): # # create new link and attached it to the previous link - # linkId = self.sim.createMultiBody(baseMass=mass, - # baseCollisionShapeIndex=collision_shape, - # baseVisualShapeIndex=visual_shape, - # basePosition=position, + # linkId = self.sim.create_body(baseMass=mass, + # base_collision_shapeIndex=collision_shape, + # base_visual_shapeIndex=visual_shape, + # position=position, # baseOrientation=orientation, # linkParentIndices=[self.joints[-1]], # linkJointTypes=[self.sim.JOINT_REVOLUTE]) @@ -85,7 +85,7 @@ class CartPole(Robot): # The values are from the cartpole URDF: https://github.com/bulletphysics/bullet3/blob/master/data/cartpole.urdf # remove body - self.sim.removeBody(self.id) + self.sim.remove_body(self.id) # create slider dims = (15, 0.025, 0.025) @@ -93,74 +93,74 @@ class CartPole(Robot): mass = 0 position = (0, 0, 0) orientation = (0, 0, 0, 1) - collisionShape = self.sim.createCollisionShape(self.sim.GEOM_BOX, halfExtents=dims) - visualShape = self.sim.createVisualShape(self.sim.GEOM_BOX, halfExtents=dims, rgbaColor=color) + collision_shape = self.sim.create_collision_shape(self.sim.GEOM_BOX, half_extents=dims) + visual_shape = self.sim.create_visual_shape(self.sim.GEOM_BOX, half_extents=dims, rgba_color=color) # create cart and pole - cartDims = (0.25, 0.25, 0.1) - cartCollisionShape = self.sim.createCollisionShape(self.sim.GEOM_BOX, halfExtents=cartDims) - cartVisualShape = self.sim.createVisualShape(self.sim.GEOM_BOX, halfExtents=cartDims, rgbaColor=(0,0,0.8,1)) - poleDims = (0.025, 0.025, 0.5) - poleCollisionShape = self.sim.createCollisionShape(self.sim.GEOM_BOX, halfExtents=poleDims) - poleVisualShape = self.sim.createVisualShape(self.sim.GEOM_BOX, halfExtents=poleDims, rgbaColor=(1,1,1,1)) + cart_dims = (0.25, 0.25, 0.1) + cart_collision_shape = self.sim.create_collision_shape(self.sim.GEOM_BOX, half_extents=cart_dims) + cart_visual_shape = self.sim.create_visual_shape(self.sim.GEOM_BOX, half_extents=cart_dims, + rgba_color=(0, 0, 0.8, 1)) + pole_dims = (0.025, 0.025, 0.5) + pole_collision_shape = self.sim.create_collision_shape(self.sim.GEOM_BOX, half_extents=pole_dims) + pole_visual_shape = self.sim.create_visual_shape(self.sim.GEOM_BOX, half_extents=pole_dims, + rgba_color=(1, 1, 1, 1)) radius = 0.05 - sphereCollisionShape = self.sim.createCollisionShape(self.sim.GEOM_SPHERE, radius=radius) - sphereVisualShape = self.sim.createVisualShape(self.sim.GEOM_SPHERE, radius=radius, rgbaColor=(1,0,0,1)) + sphere_collision_shape = self.sim.create_collision_shape(self.sim.GEOM_SPHERE, radius=radius) + sphere_visual_shape = self.sim.create_visual_shape(self.sim.GEOM_SPHERE, radius=radius, + rgba_color=(1, 0, 0, 1)) - linkMasses = [1] - linkCollisionShapeIds = [cartCollisionShape] - linkVisualShapeIds = [cartVisualShape] + link_masses = [1] + link_collision_shape_ids = [cart_collision_shape] + link_visual_shape_ids = [cart_visual_shape] - linkPositions = [[0, 0, 0]] - linkOrientations = [[0, 0, 0, 1]] - linkInertialFramePositions = [[0, 0, 0]] - linkInertialFrameOrientations = [[0, 0, 0, 1]] + link_positions = [[0, 0, 0]] + link_orientations = [[0, 0, 0, 1]] + link_inertial_frame_positions = [[0, 0, 0]] + link_inertial_frame_orientations = [[0, 0, 0, 1]] - parentIndices = [0] + parent_indices = [0] - jointTypes = [self.sim.JOINT_PRISMATIC] - jointAxis = [[1, 0, 0]] + joint_types = [self.sim.JOINT_PRISMATIC] + joint_axis = [[1, 0, 0]] # for each new link if num_links > 0: - linkMasses += [0.001, pole_mass] * num_links - linkCollisionShapeIds += [sphereCollisionShape, poleCollisionShape] * num_links - linkVisualShapeIds += [sphereVisualShape, poleVisualShape] * num_links + link_masses += [0.001, pole_mass] * num_links + link_collision_shape_ids += [sphere_collision_shape, pole_collision_shape] * num_links + link_visual_shape_ids += [sphere_visual_shape, pole_visual_shape] * num_links if inverted_pole: - linkPositions += [[0, 0, 0], [0, 0, -0.5]] - linkPositions += [[0, 0, -0.5], [0, 0, -0.5]] * (num_links - 1) + link_positions += [[0, 0, 0], [0, 0, -0.5]] + link_positions += [[0, 0, -0.5], [0, 0, -0.5]] * (num_links - 1) else: - linkPositions += [[0, 0, 0], [0, 0, 0.5]] - linkPositions += [[0, 0, 0.5], [0, 0, 0.5]] * (num_links - 1) - linkOrientations += [[0, 0, 0, 1]] * 2 * num_links - linkInertialFramePositions += [[0, 0, 0]] * 2 * num_links - linkInertialFrameOrientations += [[0, 0, 0, 1]] * 2 * num_links - parentIndices += range(1, 1 + 2 * num_links) - jointTypes += [self.sim.JOINT_REVOLUTE, self.sim.JOINT_FIXED] * num_links - jointAxis += [[0, 1, 0], [0, 1, 0]] * num_links + link_positions += [[0, 0, 0], [0, 0, 0.5]] + link_positions += [[0, 0, 0.5], [0, 0, 0.5]] * (num_links - 1) + link_orientations += [[0, 0, 0, 1]] * 2 * num_links + link_inertial_frame_positions += [[0, 0, 0]] * 2 * num_links + link_inertial_frame_orientations += [[0, 0, 0, 1]] * 2 * num_links + parent_indices += range(1, 1 + 2 * num_links) + joint_types += [self.sim.JOINT_REVOLUTE, self.sim.JOINT_FIXED] * num_links + joint_axis += [[0, 1, 0], [0, 1, 0]] * num_links # create the whole body - self.id = self.sim.createMultiBody(baseMass=mass, baseCollisionShapeIndex=collisionShape, - baseVisualShapeIndex=visualShape, basePosition=position, - baseOrientation=orientation, - baseInertialFramePosition=[0, 0, 0], - baseInertialFrameOrientation=[0, 0, 0, 1], - linkMasses=linkMasses, - linkCollisionShapeIndices=linkCollisionShapeIds, - linkVisualShapeIndices=linkVisualShapeIds, - linkPositions=linkPositions, linkOrientations=linkOrientations, - linkInertialFramePositions=linkInertialFramePositions, - linkInertialFrameOrientations=linkInertialFrameOrientations, - linkParentIndices=parentIndices, linkJointTypes=jointTypes, - linkJointAxis=jointAxis) + self.id = self.sim.create_body(mass=mass, collision_shape_id=collision_shape, visual_shape_id=visual_shape, + position=position, orientation=orientation, baseInertialFramePosition=[0, 0, 0], + baseInertialFrameOrientation=[0, 0, 0, 1], linkMasses=link_masses, + linkCollisionShapeIndices=link_collision_shape_ids, + linkVisualShapeIndices=link_visual_shape_ids, + linkPositions=link_positions, linkOrientations=link_orientations, + linkInertialFramePositions=link_inertial_frame_positions, + linkInertialFrameOrientations=link_inertial_frame_orientations, + linkParentIndices=parent_indices, linkJointTypes=joint_types, + linkJointAxis=joint_axis) # useful variables self.joints = [] # non-fixed joint/link indices in the simulator self.joint_names = {} # joint name to id in the simulator self.link_names = {} # link name to id in the simulator - for joint in range(self.getNumberOfJoints()): + for joint in range(self.num_joints): # Get joint info - jnt = self.sim.getJointInfo(self.id, joint) + jnt = self.sim.get_joint_info(self.id, joint) self.joint_names[jnt[1]] = jnt[0] self.link_names[jnt[12]] = jnt[0] # remember actuated joints @@ -168,10 +168,10 @@ class CartPole(Robot): self.joints.append(jnt[0]) # disable the joints for the pole links - # self.disableMotor(self.joints[1:]) - self.disableMotor(parentIndices[1::2]) + # self.disable_motor(self.joints[1:]) + self.disable_motor(parent_indices[1::2]) - def getSymbolicEquationsOfMotion(self, verbose=False): + def get_symbolic_equations_of_motion(self, verbose=False): """ This returns the symbolic equation of motions of the robot (using the URDF). Internally, this used the `sympy.mechanics` module. @@ -180,105 +180,109 @@ class CartPole(Robot): g, t = sympy.symbols('g t') # create the world inertial frame of reference and its origin - worldFrame = mechanics.ReferenceFrame('Fw') - worldOrigin = mechanics.Point('Pw') - worldOrigin.set_vel(worldFrame, mechanics.Vector(0)) + world_frame = mechanics.ReferenceFrame('Fw') + world_origin = mechanics.Point('Pw') + world_origin.set_vel(world_frame, mechanics.Vector(0)) # create the base frame (its position, orientation and velocities) + generalized coordinates and speeds - baseId = -1 + base_id = -1 # Check if the robot has a fixed base and create the generalized coordinates and speeds based on that, # as well the base position, orientation and velocities - if self.hasFixedBase(): + if self.has_fixed_base(): # generalized coordinates q(t) and speeds dq(t) q = mechanics.dynamicsymbols('q:{}'.format(len(self.joints))) dq = mechanics.dynamicsymbols('dq:{}'.format(len(self.joints))) - pos, orn = self.getBasePositionAndOrientation(convert_to_numpy_quaternion=False) - linVel, angVel = [0,0,0], [0,0,0] # 0 because fixed base - jointId = 0 + pos, orn = self.get_base_pose() + lin_vel, ang_vel = [0,0,0], [0,0,0] # 0 because fixed base + joint_id = 0 else: # generalized coordinates q(t) and speeds dq(t) q = mechanics.dynamicsymbols('q:{}'.format(7 + len(self.joints))) dq = mechanics.dynamicsymbols('dq:{}'.format(6 + len(self.joints))) pos, orn = q[:3], q[3:7] - linVel, angVel = dq[:3], dq[3:6] - jointId = 7 + lin_vel, ang_vel = dq[:3], dq[3:6] + joint_id = 7 # set the position, orientation and velocities of the base - baseFrame = worldFrame.orientnew('Fb', 'Quaternion', [orn[3], orn[0], orn[1], orn[2]]) - baseFrame.set_ang_vel(worldFrame, angVel[0] * worldFrame.x + angVel[1] * worldFrame.y + angVel[2] * worldFrame.z) - baseOrigin = worldOrigin.locatenew('Pb', pos[0] * worldFrame.x + pos[1] * worldFrame.y + pos[2] * worldFrame.z) - baseOrigin.set_vel(worldFrame, linVel[0] * worldFrame.x + linVel[1] * worldFrame.y + linVel[2] * worldFrame.z) + base_frame = world_frame.orientnew('Fb', 'Quaternion', [orn[3], orn[0], orn[1], orn[2]]) + base_frame.set_ang_vel(world_frame, ang_vel[0] * world_frame.x + ang_vel[1] * world_frame.y + ang_vel[2] * + world_frame.z) + base_origin = world_origin.locatenew('Pb', pos[0] * world_frame.x + pos[1] * world_frame.y + pos[2] * + world_frame.z) + base_origin.set_vel(world_frame, lin_vel[0] * world_frame.x + lin_vel[1] * world_frame.y + lin_vel[2] * + world_frame.z) # inputs u(t) (applied torques) u = mechanics.dynamicsymbols('u:{}'.format(len(self.joints))) - jointIdU = 0 + joint_id_u = 0 # kinematics differential equations kd_eqs = [q[i].diff(t) - dq[i] for i in range(len(self.joints))] # define useful lists/dicts for later bodies, loads = [], [] - frames = {baseId: (baseFrame, baseOrigin)} - # frames = {baseId: (worldFrame, worldOrigin)} + frames = {base_id: (base_frame, base_origin)} + # frames = {base_id: (worldFrame, worldOrigin)} # go through each joint/link (each link is associated to a joint) - for linkId in range(self.getNumberOfLinks()): + for link_id in range(self.num_links): # get useful information about joint/link kinematics and dynamics from simulator - info = self.sim.getDynamicsInfo(self.id, linkId) - mass, localInertiaDiagonal = info[0], np.array(info[2]) - info = self.sim.getLinkState(self.id, linkId) - localInertialFramePosition, localInertialFrameOrientation = info[2], info[3] + info = self.sim.get_dynamics_info(self.id, link_id) + mass, local_inertia_diagonal = info[0], info[2] + info = self.sim.get_link_state(self.id, link_id) + local_inertial_frame_position, local_inertial_frame_orientation = info[2], info[3] # worldLinkFramePosition, worldLinkFrameOrientation = info[4], info[5] - info = self.sim.getJointInfo(self.id, linkId) - jointName, jointType = info[1:3] + info = self.sim.get_joint_info(self.id, link_id) + joint_name, joint_type = info[1:3] # jointDamping, jointFriction = info[6:8] - linkName, jointAxisInLocalFrame, parentFramePosition, parentFrameOrientation, parentIdx = info[-5:] - xl, yl, zl = jointAxisInLocalFrame + link_name, joint_axis_in_local_frame, parent_frame_position, parent_frame_orientation, \ + parent_idx = info[-5:] + xl, yl, zl = joint_axis_in_local_frame # get previous references - parentFrame, parentPoint = frames[parentIdx] + parent_frame, parent_point = frames[parent_idx] # create a reference frame with its origin for each joint # set frame orientation - if jointType == self.sim.JOINT_REVOLUTE: - R = np.array(self.sim.getMatrixFromQuaternion(parentFrameOrientation)).reshape(3,3) - R1 = getSymbolicMatrixFromAxisAngle(jointAxisInLocalFrame, q[jointId]) + if joint_type == self.sim.JOINT_REVOLUTE: + R = get_matrix_from_quaternion(parent_frame_orientation) + R1 = get_symbolic_matrix_from_axis_angle(joint_axis_in_local_frame, q[joint_id]) R = R1.dot(R) - frame = parentFrame.orientnew('F' + str(linkId), 'DCM', sympy.Matrix(R)) + frame = parent_frame.orientnew('F' + str(link_id), 'DCM', sympy.Matrix(R)) else: - x, y, z, w = parentFrameOrientation # orientation of the joint in parent CoM inertial frame - frame = parentFrame.orientnew('F' + str(linkId), 'Quaternion', [w, x, y, z]) + x, y, z, w = parent_frame_orientation # orientation of the joint in parent CoM inertial frame + frame = parent_frame.orientnew('F' + str(link_id), 'Quaternion', [w, x, y, z]) # set frame angular velocity - angVel = 0 - if jointType == self.sim.JOINT_REVOLUTE: - angVel = dq[jointId] * (xl * frame.x + yl * frame.y + zl * frame.z) - frame.set_ang_vel(parentFrame, angVel) + ang_vel = 0 + if joint_type == self.sim.JOINT_REVOLUTE: + ang_vel = dq[joint_id] * (xl * frame.x + yl * frame.y + zl * frame.z) + frame.set_ang_vel(parent_frame, ang_vel) # create origin of the reference frame # set origin position - x, y, z = parentFramePosition # position of the joint in parent CoM inertial frame - pos = x * parentFrame.x + y * parentFrame.y + z * parentFrame.z - if jointType == self.sim.JOINT_PRISMATIC: - pos += q[jointId] * (xl * frame.x + yl * frame.y + zl * frame.z) - origin = parentPoint.locatenew('P' + str(linkId), pos) + x, y, z = parent_frame_position # position of the joint in parent CoM inertial frame + pos = x * parent_frame.x + y * parent_frame.y + z * parent_frame.z + if joint_type == self.sim.JOINT_PRISMATIC: + pos += q[joint_id] * (xl * frame.x + yl * frame.y + zl * frame.z) + origin = parent_point.locatenew('P' + str(link_id), pos) # set origin velocity - if jointType == self.sim.JOINT_PRISMATIC: - vel = dq[jointId] * (xl * frame.x + yl * frame.y + zl * frame.z) - origin.set_vel(worldFrame, vel.express(worldFrame)) + if joint_type == self.sim.JOINT_PRISMATIC: + vel = dq[joint_id] * (xl * frame.x + yl * frame.y + zl * frame.z) + origin.set_vel(world_frame, vel.express(world_frame)) else: - origin.v2pt_theory(parentPoint, worldFrame, parentFrame) + origin.v2pt_theory(parent_point, world_frame, parent_frame) # define CoM frame and position (and velocities) wrt the local link frame - x, y, z, w = localInertialFrameOrientation - com_frame = frame.orientnew('Fc' + str(linkId), 'Quaternion', [w, x, y, z]) + x, y, z, w = local_inertial_frame_orientation + com_frame = frame.orientnew('Fc' + str(link_id), 'Quaternion', [w, x, y, z]) com_frame.set_ang_vel(frame, mechanics.Vector(0)) - x, y, z = localInertialFramePosition - com = origin.locatenew('C' + str(linkId), x * frame.x + y * frame.y + z * frame.z) - com.v2pt_theory(origin, worldFrame, frame) + x, y, z = local_inertial_frame_position + com = origin.locatenew('C' + str(link_id), x * frame.x + y * frame.y + z * frame.z) + com.v2pt_theory(origin, world_frame, frame) # define com particle # com_particle = mechanics.Particle('Pa' + str(linkId), com, mass) @@ -287,62 +291,62 @@ class CartPole(Robot): # save # frames[linkId] = (frame, origin) # frames[linkId] = (frame, origin, com_frame, com) - frames[linkId] = (com_frame, com) + frames[link_id] = (com_frame, com) # define mass and inertia - ixx, iyy, izz = localInertiaDiagonal + ixx, iyy, izz = local_inertia_diagonal inertia = mechanics.inertia(com_frame, ixx, iyy, izz, ixy=0, iyz=0, izx=0) inertia = (inertia, com) # define rigid body associated to frame - body = mechanics.RigidBody(linkName, com, frame, mass, inertia) + body = mechanics.RigidBody(link_name, com, frame, mass, inertia) bodies.append(body) # define dynamical forces/torques acting on the body # gravity force applied on the CoM - force = (com, - mass * g * worldFrame.z) + force = (com, - mass * g * world_frame.z) loads.append(force) # if prismatic joint, compute force - if jointType == self.sim.JOINT_PRISMATIC: - force = (origin, u[jointIdU] * (xl * frame.x + yl * frame.y + zl * frame.z)) + if joint_type == self.sim.JOINT_PRISMATIC: + force = (origin, u[joint_id_u] * (xl * frame.x + yl * frame.y + zl * frame.z)) # force = (com, u[jointIdU] * (x * frame.x + y * frame.y + z * frame.z) - mass * g * worldFrame.z) loads.append(force) # if revolute joint, compute torque - if jointType == self.sim.JOINT_REVOLUTE: + if joint_type == self.sim.JOINT_REVOLUTE: v = (xl * frame.x + yl * frame.y + zl * frame.z) # torqueOnPrevBody = (parentFrame, - u[jointIdU] * v) - torqueOnPrevBody = (parentFrame, - u[jointIdU] * v) - torqueOnCurrBody = (frame, u[jointIdU] * v) - loads.append(torqueOnPrevBody) - loads.append(torqueOnCurrBody) + torque_on_prev_body = (parent_frame, - u[joint_id_u] * v) + torque_on_curr_body = (frame, u[joint_id_u] * v) + loads.append(torque_on_prev_body) + loads.append(torque_on_curr_body) # if joint is not fixed increment the current joint id - if jointType != self.sim.JOINT_FIXED: - jointId += 1 - jointIdU += 1 + if joint_type != self.sim.JOINT_FIXED: + joint_id += 1 + joint_id_u += 1 if verbose: - print("\nLink name with type: {} - {}".format(linkName, self.getJointTypes(jointId=linkId))) + print("\nLink name with type: {} - {}".format(link_name, self.get_joint_types(joint_ids=link_id))) print("------------------------------------------------------") - print("Position of joint frame wrt parent frame: {}".format(origin.pos_from(parentPoint))) - print("Orientation of joint frame wrt parent frame: {}".format(frame.dcm(parentFrame))) - print("Linear velocity of joint frame wrt parent frame: {}".format(origin.vel(worldFrame).express(parentFrame))) - print("Angular velocity of joint frame wrt parent frame: {}".format(frame.ang_vel_in(parentFrame))) + print("Position of joint frame wrt parent frame: {}".format(origin.pos_from(parent_point))) + print("Orientation of joint frame wrt parent frame: {}".format(frame.dcm(parent_frame))) + print("Linear velocity of joint frame wrt parent frame: {}".format(origin.vel(world_frame).express(parent_frame))) + print("Angular velocity of joint frame wrt parent frame: {}".format(frame.ang_vel_in(parent_frame))) print("------------------------------------------------------") - print("Position of joint frame wrt world frame: {}".format(origin.pos_from(worldOrigin))) - print("Orientation of joint frame wrt world frame: {}".format(frame.dcm(worldFrame).simplify())) - print("Linear velocity of joint frame wrt world frame: {}".format(origin.vel(worldFrame))) - print("Angular velocity of joint frame wrt parent frame: {}".format(frame.ang_vel_in(worldFrame))) + print("Position of joint frame wrt world frame: {}".format(origin.pos_from(world_origin))) + print("Orientation of joint frame wrt world frame: {}".format(frame.dcm(world_frame).simplify())) + print("Linear velocity of joint frame wrt world frame: {}".format(origin.vel(world_frame))) + print("Angular velocity of joint frame wrt parent frame: {}".format(frame.ang_vel_in(world_frame))) print("------------------------------------------------------") # print("Local position of CoM wrt joint frame: {}".format(com.pos_from(origin))) # print("Local linear velocity of CoM wrt joint frame: {}".format(com.vel(worldFrame).express(frame))) # print("Local angular velocity of CoM wrt joint frame: {}".format(com_frame.ang_vel_in(frame))) # print("------------------------------------------------------") - if jointType == self.sim.JOINT_PRISMATIC: + if joint_type == self.sim.JOINT_PRISMATIC: print("Input value (force): {}".format(loads[-1])) - elif jointType == self.sim.JOINT_REVOLUTE: + elif joint_type == self.sim.JOINT_REVOLUTE: print("Input value (torque on previous and current bodies): {} and {}".format(loads[-2], loads[-1])) print("") @@ -360,7 +364,7 @@ class CartPole(Robot): # TODO: 2. account for constraints (e.g. holonomic, non-holonomic, etc.) # Get the Equation of Motion (EoM) using Kane's method - kane = mechanics.KanesMethod(worldFrame, q_ind=q, u_ind=dq, kd_eqs=kd_eqs) + kane = mechanics.KanesMethod(world_frame, q_ind=q, u_ind=dq, kd_eqs=kd_eqs) kane.kanes_equations(bodies=bodies, loads=loads) # get mass matrix and force vector (after simplifying) such that :math:`M(x,t) \dot{x} = f(x,t)` @@ -387,7 +391,7 @@ class CartPole(Robot): return M, f - def linearizeEquationOfMotion(self, point=None, verbose=False): + def linearize_equations_of_motion(self, point=None, verbose=False): """ Linearize the equation of motions around the given point (=state). That is, instead of having :math:`\dot{x} = f(x,u)` where :math:`f` is in general a non-linear function, it linearizes it around @@ -399,10 +403,10 @@ class CartPole(Robot): the matrices. """ if self.symbols is None: - self.getSymbolicEquationsOfMotion() + self.get_symbolic_equations_of_motion() if point is None: # take current state - point = list(self.getJointPositions()) + list(self.getJointVelocities()) + point = list(self.get_joint_positions()) + list(self.get_joint_velocities()) point = dict(zip(self.symbols['q'] + self.symbols['dq'], point)) kane, parameters = self.symbols['kane'], self.symbols['parameters'] @@ -530,12 +534,12 @@ if __name__ == "__main__": robot = CartPole(sim, num_links=num_links) # print information about the robot - robot.printRobotInfo() + robot.print_info() - robot.getSymbolicEquationsOfMotion() + robot.get_symbolic_equations_of_motion() eq_point = np.zeros((num_links + 1) * 2) # state = [q, dq] - A, B = robot.linearizeEquationOfMotion(eq_point) + A, B = robot.linearize_equations_of_motion(eq_point) # LQR controller lqr = LQR(A, B) @@ -543,9 +547,9 @@ if __name__ == "__main__": for i in count(): # control - x = np.concatenate((robot.getJointPositions(), robot.getJointVelocities())) + x = np.concatenate((robot.get_joint_positions(), robot.get_joint_velocities())) u = K.dot(eq_point - x) - robot.setJointTorques(u[0], 0) + robot.set_joint_torques(u[0], 0) print("U[0] = {}".format(u[0])) diff --git a/pyrobolearn/robots/cassie.py b/pyrobolearn/robots/cassie.py index 33e6149..bceb7ec 100644 --- a/pyrobolearn/robots/cassie.py +++ b/pyrobolearn/robots/cassie.py @@ -25,40 +25,40 @@ class Cassie(BipedRobot): def __init__(self, simulator, - init_pos=(0, 0, .8), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, .8), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/cassie/cassie.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/cassie/cassie.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.8) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.8,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.8) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.8,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Cassie, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Cassie, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'cassie' # TODO: create constraints in pybullet # From the user guide: "URDF, SDF and MJCF specify articulated bodies as a tree-structures without loops. # The 'createConstraint' allows you to connect specific links of bodies to close those loops." - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['left_pelvis_rotation', 'left_hip', 'left_thigh', 'left_knee', 'left_shin', 'left_tarsus', 'left_toe'], ['right_pelvis_rotation', 'right_hip', 'right_thigh', 'right_knee', 'right_shin', 'right_tarsus', 'right_toe']]] - self.feet = [self.getLinkIds(link) for link in ['left_toe', 'right_toe'] if link in self.link_names] + self.feet = [self.get_link_ids(link) for link in ['left_toe', 'right_toe'] if link in self.link_names] # set joint angles to home position - self.setJointHomePositions() + self.set_joint_home_positions() - def getHomeJointPositions(self): + def get_home_joint_positions(self): """Return the joint positions for the home position""" return np.array([0, 0, 1.0204, -1.97, -0.084, 2.06, -1.9, 0, 0, 1.0204, -1.97, -0.084, 2.06, -1.9]) @@ -78,13 +78,13 @@ if __name__ == "__main__": robot = Cassie(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - robot.moveJointHomePositions() + # robot.update_joint_slider() + robot.move_joint_home_positions() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/centauro.py b/pyrobolearn/robots/centauro.py index 3cea57e..ab96ee6 100644 --- a/pyrobolearn/robots/centauro.py +++ b/pyrobolearn/robots/centauro.py @@ -14,35 +14,35 @@ class Centauro(WheeledRobot, QuadrupedRobot, BiManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 1.), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 1.), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/centauro/centauro_stick.urdf' + urdf=os.path.dirname(__file__) + '/urdfs/centauro/centauro_stick.urdf' # centauro_stick.urdf, centauro_soft_hand.urdf, centauro_heri.urdf, # centauro_schunk_handL.urdf, centauro_schunk_hand.urdf ): # check parameters - if init_pos is None: - init_pos = (0., 0., 1.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (1.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 1.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (1.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Centauro, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase) + super(Centauro, self).__init__(simulator, urdf, position, orientation, fixed_base) self.name = 'centauro' - self.necks = [self.getLinkIds(link) for link in ['neck_'+str(i) for i in range(1, 4)]] + self.necks = [self.get_link_ids(link) for link in ['neck_' + str(i) for i in range(1, 4)]] # define wheels - self.wheels = [self.getLinkIds(link) for link in ['wheel_'+str(i) for i in range(1, 5)]] + self.wheels = [self.get_link_ids(link) for link in ['wheel_' + str(i) for i in range(1, 5)]] self.wheel_directions = np.array([-1., 1., -1., 1]) # define legs and feet - self.legs = [[self.getLinkIds(link) for link in links] + self.legs = [[self.get_link_ids(link) for link in links] for links in [['hip1_1', 'hip2_1', 'knee_1', 'ankle1_1', 'ankle2_1', 'wheel_1'], ['hip1_2', 'hip2_2', 'knee_2', 'ankle1_2', 'ankle2_2', 'wheel_2'], ['hip1_3', 'hip2_3', 'knee_3', 'ankle1_3', 'ankle2_3', 'wheel_3'], @@ -51,11 +51,11 @@ class Centauro(WheeledRobot, QuadrupedRobot, BiManipulatorRobot): self.feet = self.wheels # define arms and hands - self.arms = [[self.getLinkIds(link) for link in links] + self.arms = [[self.get_link_ids(link) for link in links] for links in [['arm1_1', 'arm1_2', 'arm1_3', 'arm1_4', 'arm1_5', 'arm1_6', 'arm1_7'], ['arm2_1', 'arm2_2', 'arm2_3', 'arm2_4', 'arm2_5', 'arm2_6', 'arm2_7']]] - self.hands = [self.getLinkIds(link) for link in ['arm1_8', 'arm2_8']] + self.hands = [self.get_link_ids(link) for link in ['arm1_8', 'arm2_8']] # Test @@ -74,14 +74,14 @@ if __name__ == "__main__": robot = Centauro(sim) # , useFixedBase=True) # print information about the robot - robot.printRobotInfo() + robot.print_info() print("Number of Legs: {}".format(robot.getNumberOfLegs())) print("Number of Arms: {}".format(robot.getNumberOfArms())) - # robot.addJointSlider(robot.getRightFrontLegIds()) + # robot.add_joint_slider(robot.getRightFrontLegIds()) robot.drive(speed=3) # run simulator for _ in count(): - robot.updateJointSlider() + robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/cogimon.py b/pyrobolearn/robots/cogimon.py index 1ad519f..8859b5a 100644 --- a/pyrobolearn/robots/cogimon.py +++ b/pyrobolearn/robots/cogimon.py @@ -15,49 +15,49 @@ class Cogimon(BipedRobot, BiManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 1.), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 1.), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/cogimon/cogimon.urdf', + urdf=os.path.dirname(__file__) + '/urdfs/cogimon/cogimon.urdf', lower_body=False): # cogimon_lower_body.urdf # check parameters - if init_pos is None: - init_pos = (0., 0., 1.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (1.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 1.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (1.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False if lower_body: - urdf_path = os.path.dirname(__file__) + '/urdfs/cogimon/cogimon_lower_body.urdf' + urdf = os.path.dirname(__file__) + '/urdfs/cogimon/cogimon_lower_body.urdf' - super(Cogimon, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Cogimon, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'cogimon' - self.waist = self.getLinkIds('DWL') if 'DWL' in self.link_names else None - self.torso = self.getLinkIds('DWYTorso') if 'DWYTorso' in self.link_names else None + self.waist = self.get_link_ids('DWL') if 'DWL' in self.link_names else None + self.torso = self.get_link_ids('DWYTorso') if 'DWYTorso' in self.link_names else None - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['LHipMot', 'LThighUpLeg', 'LThighLowLeg', 'LLowLeg', 'LFootmot', 'LFoot'], ['RHipMot', 'RThighUpLeg', 'RThighLowLeg', 'RLowLeg', 'RFootmot', 'RFoot']]] - self.feet = [self.getLinkIds(link) for link in ['LFoot', 'RFoot'] if link in self.link_names] + self.feet = [self.get_link_ids(link) for link in ['LFoot', 'RFoot'] if link in self.link_names] - self.arms = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.arms = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['LShp', 'LShr', 'LShy', 'LElb', 'LForearm', 'LWrMot2', 'LWrMot3'], ['RShp', 'RShr', 'RShy', 'RElb', 'RForearm', 'RWrMot2', 'RWrMot3']]] - self.hands = [self.getLinkIds(link) for link in ['LSoftHand', 'RSoftHand'] if link in self.link_names] + self.hands = [self.get_link_ids(link) for link in ['LSoftHand', 'RSoftHand'] if link in self.link_names] def CogimonLowerBody(simulator, init_pos=(0, 0, 1.), init_orient=(0, 0, 0, 1), useFixedBase=False, scaling=1., urdf_path=os.path.dirname(__file__) + '/urdfs/cogimon/cogimon_lower_body.urdf'): """Load Cogimon Lower Body""" - return Cogimon(simulator=simulator, init_pos=init_pos, init_orient=init_orient, useFixedBase=useFixedBase, - scaling=scaling, urdf_path=urdf_path) + return Cogimon(simulator=simulator, position=init_pos, orientation=init_orient, fixed_base=useFixedBase, + scaling=scaling, urdf=urdf_path) # Test @@ -76,12 +76,12 @@ if __name__ == "__main__": robot = Cogimon(sim, lower_body=False) # print information about the robot - robot.printRobotInfo() + robot.print_info() # # Position control using sliders - robot.addJointSlider(robot.left_leg) + robot.add_joint_slider(robot.left_leg) # run simulator for _ in count(): - robot.updateJointSlider() + robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/coman.py b/pyrobolearn/robots/coman.py index d9693d1..21fe811 100644 --- a/pyrobolearn/robots/coman.py +++ b/pyrobolearn/robots/coman.py @@ -16,42 +16,42 @@ class Coman(BipedRobot, BiManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.5), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.5), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/coman/coman.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/coman/coman.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.5) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.5,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.5) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.5,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Coman, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Coman, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'coman' - self.torso = [self.getLinkIds(link) for link in ['DWL', 'DWS', 'DWYTorso'] if link in self.link_names] + self.torso = [self.get_link_ids(link) for link in ['DWL', 'DWS', 'DWYTorso'] if link in self.link_names] - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['LHipMot', 'LThighUpLeg', 'LThighLowLeg', 'LLowLeg', 'LFootmot', 'LFoot'], ['RHipMot', 'RThighUpLeg', 'RThighLowLeg', 'RLowLeg', 'RFootmot', 'RFoot']]] - self.feet = [self.getLinkIds(link) for link in ['LFoot', 'RFoot'] if link in self.link_names] + self.feet = [self.get_link_ids(link) for link in ['LFoot', 'RFoot'] if link in self.link_names] - self.arms = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.arms = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['LShp', 'LShr', 'LShy', 'LElb', 'LForearm', 'LWrMot2', 'LWrMot3'], ['RShp', 'RShr', 'RShy', 'RElb', 'RForearm', 'RWrMot2', 'RWrMot3']]] - self.hands = [self.getLinkIds(link) for link in ['LSoftHand', 'RSoftHand'] if link in self.link_names] + self.hands = [self.get_link_ids(link) for link in ['LSoftHand', 'RSoftHand'] if link in self.link_names] # move a little bit the arms - # self.setJointPositions([-np.pi/12, np.pi/12], [self.joints[i] for i in [4,11]]) # shoulder - # self.setJointPositions([-np.pi / 4, -np.pi / 4], [self.joints[i] for i in [6, 13]]) # elbow - # self.setJointPositions([-np.pi/4, -np.pi/4], [self.joints[i] for i in [17,23]]) + # self.set_joint_positions([-np.pi/12, np.pi/12], [self.joints[i] for i in [4,11]]) # shoulder + # self.set_joint_positions([-np.pi / 4, -np.pi / 4], [self.joints[i] for i in [6, 13]]) # elbow + # self.set_joint_positions([-np.pi/4, -np.pi/4], [self.joints[i] for i in [17,23]]) # for _ in range(10): # self.sim.stepSimulation() @@ -69,23 +69,23 @@ if __name__ == "__main__": world = BasicWorld(sim) # create robot - robot = Coman(sim, useFixedBase=True) + robot = Coman(sim, fixed_base=True) # print information about the robot - robot.printRobotInfo() + robot.print_info() print(robot.link_names) # # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() - robot.changeTransparency() - # robot.drawLinkCoMs() - robot.drawLinkFrames() - # robot.drawBoundingBoxes(robot.right_leg[4]) + robot.change_transparency() + # robot.draw_link_coms() + robot.draw_link_frames() + # robot.draw_bounding_boxes(robot.right_leg[4]) # run simulator for _ in count(): - # robot.updateJointSlider() - # robot.computeAndDrawCoMPosition() - # robot.computeAndDrawProjectedCoMPosition() + # robot.update_joint_slider() + # robot.compute_and_draw_com_position() + # robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/crab.py b/pyrobolearn/robots/crab.py index 148b609..6df80ac 100644 --- a/pyrobolearn/robots/crab.py +++ b/pyrobolearn/robots/crab.py @@ -17,25 +17,25 @@ class Crab(HexapodRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.12), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.12), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/crab/crab.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/crab/crab.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.12) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.12,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.12) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.12,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Crab, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Crab, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'crab' - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['coxa_l1', 'femur_l1', 'tibia_l1'], [ 'coxa_r1', 'femur_r1', 'tibia_r1'], ['coxa_l2', 'femur_l2', 'tibia_l2'], @@ -43,7 +43,7 @@ class Crab(HexapodRobot): ['coxa_l3', 'femur_l3', 'tibia_l3'], ['coxa_r3', 'femur_r3', 'tibia_r3']]] - self.feet = [self.getLinkIds(link) for link in ['tibia_foot_l1', 'tibia_foot_r1', 'tibia_foot_l2', + self.feet = [self.get_link_ids(link) for link in ['tibia_foot_l1', 'tibia_foot_r1', 'tibia_foot_l2', 'tibia_foot_r2', 'tibia_foot_l3', 'tibia_foot_r3'] if link in self.link_names] @@ -64,13 +64,13 @@ if __name__ == "__main__": robot = Crab(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - robot.addJointSlider(robot.right_middle_leg) + robot.add_joint_slider(robot.right_middle_leg) # run simulation for i in count(): - robot.updateJointSlider() + robot.update_joint_slider() # step in simulation world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/cubli.py b/pyrobolearn/robots/cubli.py index a5f8a29..5121bc2 100644 --- a/pyrobolearn/robots/cubli.py +++ b/pyrobolearn/robots/cubli.py @@ -24,26 +24,26 @@ class Cubli(Robot): def __init__(self, simulator, - init_pos=(0, 0, 0.5), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.5), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/cubli/cubli.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/cubli/cubli.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.5) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.5,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.5) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.5,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Cubli, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Cubli, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'cubli' # disable each motor joint - self.disableMotor() + self.disable_motor() # Test @@ -66,8 +66,8 @@ if __name__ == "__main__": robot = Cubli(sim, position, orientation, scaling=scale) # print information about the robot - robot.printRobotInfo() - H = robot.calculateMassMatrix(qIdx=slice(6, 6+len(robot.joints))) # floating base, thus keep only the last q + robot.print_info() + H = robot.get_mass_matrix(q_idx=slice(6, 6 + len(robot.joints))) # floating base, thus keep only the last q print("Inertia matrix: H(q) = {}\n".format(H)) # PD control @@ -77,13 +77,13 @@ if __name__ == "__main__": for i in count(): # get state - quaternion = robot.getBaseOrientation(False) - w = robot.getBaseAngularVelocity() + quaternion = robot.get_base_orientation(False) + w = robot.get_base_angular_velocity() euler = pybullet.getEulerFromQuaternion(quaternion.tolist()) # PD control torques = [-Kp * (desired_roll - euler[0]) + Kd * w[0], 0., 0.] - robot.setJointTorques(torques) + robot.set_joint_torques(torques) # step in simulation world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/darwin.py b/pyrobolearn/robots/darwin.py index 694c957..b113a30 100644 --- a/pyrobolearn/robots/darwin.py +++ b/pyrobolearn/robots/darwin.py @@ -18,42 +18,42 @@ class Darwin(BipedRobot, BiManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.34), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.34), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/darwin/darwin.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/darwin/darwin.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.34) - if len(init_pos) == 2: # assume x, y are given - init_pos += (0.34,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.34) + if len(position) == 2: # assume x, y are given + position += (0.34,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Darwin, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Darwin, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'darwin' - # self.torso = [self.getLinkIds(link) for link in ['DWL', 'DWS', 'DWYTorso'] if link in self.link_names] + # self.torso = [self.get_link_ids(link) for link in ['DWL', 'DWS', 'DWYTorso'] if link in self.link_names] # - # self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + # self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] # for links in [['LHipMot', 'LThighUpLeg', 'LThighLowLeg', 'LLowLeg', 'LFootmot', 'LFoot'], # ['RHipMot', 'RThighUpLeg', 'RThighLowLeg', 'RLowLeg', 'RFootmot', 'RFoot']]] # - # self.feet = [self.getLinkIds(link) for link in ['LFoot', 'RFoot'] if link in self.link_names] + # self.feet = [self.get_link_ids(link) for link in ['LFoot', 'RFoot'] if link in self.link_names] # - # self.arms = [[self.getLinkIds(link) for link in links if link in self.link_names] + # self.arms = [[self.get_link_ids(link) for link in links if link in self.link_names] # for links in [['LShp', 'LShr', 'LShy', 'LElb', 'LForearm', 'LWrMot2', 'LWrMot3'], # ['RShp', 'RShr', 'RShy', 'RElb', 'RForearm', 'RWrMot2', 'RWrMot3']]] # - # self.hands = [self.getLinkIds(link) for link in ['LSoftHand', 'RSoftHand'] if link in self.link_names] + # self.hands = [self.get_link_ids(link) for link in ['LSoftHand', 'RSoftHand'] if link in self.link_names] # move a little bit the arms - # self.setJointPositions([-np.pi/12, np.pi/12], [self.joints[i] for i in [4,11]]) # shoulder - # self.setJointPositions([-np.pi / 4, -np.pi / 4], [self.joints[i] for i in [6, 13]]) # elbow - # self.setJointPositions([-np.pi/4, -np.pi/4], [self.joints[i] for i in [17,23]]) + # self.set_joint_positions([-np.pi/12, np.pi/12], [self.joints[i] for i in [4,11]]) # shoulder + # self.set_joint_positions([-np.pi / 4, -np.pi / 4], [self.joints[i] for i in [6, 13]]) # elbow + # self.set_joint_positions([-np.pi/4, -np.pi/4], [self.joints[i] for i in [17,23]]) # for _ in range(10): # self.sim.stepSimulation() @@ -71,16 +71,16 @@ if __name__ == "__main__": world = BasicWorld(sim) # create robot - robot = Darwin(sim, useFixedBase=False) + robot = Darwin(sim, fixed_base=False) # print information about the robot - robot.printRobotInfo() + robot.print_info() print(robot.link_names) # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/edo.py b/pyrobolearn/robots/edo.py index 65d8372..b8df219 100644 --- a/pyrobolearn/robots/edo.py +++ b/pyrobolearn/robots/edo.py @@ -20,22 +20,22 @@ class Edo(ManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), - useFixedBase=True, + position=(0, 0, 0), + orientation=(0, 0, 0, 1), + fixed_base=True, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/edo/edo.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/edo/edo.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True - super(Edo, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Edo, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'edo' @@ -55,8 +55,8 @@ if __name__ == "__main__": robot = Edo(sim) # print information about the robot - robot.printRobotInfo() - # H = robot.calculateMassMatrix() + robot.print_info() + # H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) for i in count(): diff --git a/pyrobolearn/robots/epuck.py b/pyrobolearn/robots/epuck.py index 9cc8dbe..67a5dc9 100644 --- a/pyrobolearn/robots/epuck.py +++ b/pyrobolearn/robots/epuck.py @@ -19,25 +19,25 @@ class Epuck(DifferentialWheeledRobot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/epuck/epuck.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/epuck/epuck.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.0,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.0,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Epuck, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Epuck, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'epuck' - self.wheels = [self.getLinkIds(link) for link in ['left_wheel', 'right_wheel'] + self.wheels = [self.get_link_ids(link) for link in ['left_wheel', 'right_wheel'] if link in self.link_names] self.wheel_directions = np.ones(len(self.wheels)) @@ -58,18 +58,18 @@ if __name__ == "__main__": robots = [] for _ in range(5): x, y = np.random.uniform(low=-2, high=2, size=2) - robot = world.loadRobot(Epuck, position=(x, y, 0)) + robot = world.load_robot(Epuck, position=(x, y, 0)) robots.append(robot) # print information about the robot - robots[0].printRobotInfo() + robots[0].print_info() # Position control using sliders - # robots[0].addJointSlider() + # robots[0].add_joint_slider() # run simulator for _ in count(): - # robots[0].updateJointSlider() + # robots[0].update_joint_slider() for robot in robots: robot.drive(5) world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/f10_racecar.py b/pyrobolearn/robots/f10_racecar.py index 7e69708..98833f7 100644 --- a/pyrobolearn/robots/f10_racecar.py +++ b/pyrobolearn/robots/f10_racecar.py @@ -17,36 +17,36 @@ class F10Racecar(AckermannWheeledRobot): def __init__(self, simulator, - init_pos=(0, 0, .1), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, .1), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/f10_racecar/racecar.urdf'): # racecar_differential.urdf + urdf=os.path.dirname(__file__) + '/urdfs/f10_racecar/racecar.urdf'): # racecar_differential.urdf # check parameters - if init_pos is None: - init_pos = (0., 0., 0.1) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.1,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.1) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.1,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(F10Racecar, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(F10Racecar, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'racecar' - self.wheels = [self.getLinkIds(link) for link in ['left_front_wheel', 'right_front_wheel', + self.wheels = [self.get_link_ids(link) for link in ['left_front_wheel', 'right_front_wheel', 'left_rear_wheel', 'right_rear_wheel'] if link in self.link_names] self.wheel_directions = np.ones(len(self.wheels)) - self.steering = [self.getLinkIds(link) for link in ['left_steering_hinge', 'right_steering_hinge'] + self.steering = [self.get_link_ids(link) for link in ['left_steering_hinge', 'right_steering_hinge'] if link in self.link_names] - def setSteering(self, angle): + def set_steering(self, angle): """Set steering angle""" angle = angle * np.ones(len(self.steering)) - self.setJointPositions(angle, jointId=self.steering) + self.set_joint_positions(angle, joint_ids=self.steering) # Test @@ -65,13 +65,13 @@ if __name__ == "__main__": robot = F10Racecar(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - robot.driveForward(10) + # robot.update_joint_slider() + robot.drive_forward(10) world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/fetch.py b/pyrobolearn/robots/fetch.py index ac3cee5..ff5e47c 100644 --- a/pyrobolearn/robots/fetch.py +++ b/pyrobolearn/robots/fetch.py @@ -15,22 +15,22 @@ class Fetch(WheeledRobot, ManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.1), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.1), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/fetch/fetch.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/fetch/fetch.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.1) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.1,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.1) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.1,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Fetch, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Fetch, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'fetch' @@ -50,12 +50,12 @@ if __name__ == "__main__": robot = Fetch(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - robot.addJointSlider() + robot.add_joint_slider() # run simulator for _ in count(): - robot.updateJointSlider() + robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/franka.py b/pyrobolearn/robots/franka.py index c6cd37c..f768fd9 100644 --- a/pyrobolearn/robots/franka.py +++ b/pyrobolearn/robots/franka.py @@ -21,25 +21,25 @@ class Franka(ManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), + position=(0, 0, 0), + orientation=(0, 0, 0, 1), scaling=1., - useFixedBase=True, - urdf_path=os.path.dirname(__file__) + '/urdfs/franka/franka.urdf'): + fixed_base=True, + urdf=os.path.dirname(__file__) + '/urdfs/franka/franka.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.0,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.0,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True - super(Franka, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Franka, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'franka' - # self.disableMotor() + # self.disable_motor() # Test @@ -58,14 +58,14 @@ if __name__ == "__main__": robot = Franka(sim) # print information about the robot - robot.printRobotInfo() - # H = robot.calculateMassMatrix() + robot.print_info() + # H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() for i in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() # step in simulation world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/half_cheetah.py b/pyrobolearn/robots/half_cheetah.py index 0b02621..bb4f842 100644 --- a/pyrobolearn/robots/half_cheetah.py +++ b/pyrobolearn/robots/half_cheetah.py @@ -13,22 +13,22 @@ class HalfCheetah(Robot): def __init__(self, simulator, - init_pos=(-0.5, 0, 0.1), - init_orient=(0, 0.707, 0, 0.707), - useFixedBase=False, + position=(-0.5, 0, 0.1), + orientation=(0, 0.707, 0, 0.707), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/mjcfs/half_cheetah.xml'): + urdf=os.path.dirname(__file__) + '/mjcfs/half_cheetah.xml'): # check parameters - if init_pos is None: - init_pos = (-0.5, 0., 0.1) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.1,) - if init_orient is None: - init_orient = (0, 0.707, 0, 0.707) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (-0.5, 0., 0.1) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.1,) + if orientation is None: + orientation = (0, 0.707, 0, 0.707) + if fixed_base is None: + fixed_base = False - super(HalfCheetah, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(HalfCheetah, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'halfCheetah' @@ -48,7 +48,7 @@ if __name__ == "__main__": robot = HalfCheetah(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # run simulation for i in count(): diff --git a/pyrobolearn/robots/hand.py b/pyrobolearn/robots/hand.py index b8d0893..03b8e00 100644 --- a/pyrobolearn/robots/hand.py +++ b/pyrobolearn/robots/hand.py @@ -12,12 +12,12 @@ class Hand(Robot): def __init__(self, simulator, - urdf_path, - init_pos=(0, 0, 1.), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + urdf, + position=(0, 0, 1.), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1.): - super(Hand, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Hand, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.fingers = [] # list of fingers where each finger is a list of links/joints @@ -39,12 +39,12 @@ class TwoHand(Hand): """ def __init__(self, simulator, - urdf_path, - init_pos=(0, 0, 1.), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + urdf, + position=(0, 0, 1.), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1.): - super(TwoHand, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(TwoHand, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.left_fingers = [] # list of ids in self.fingers self.right_fingers = [] # list of ids in self.fingers diff --git a/pyrobolearn/robots/hopper.py b/pyrobolearn/robots/hopper.py index c7a05a4..b08ff0d 100644 --- a/pyrobolearn/robots/hopper.py +++ b/pyrobolearn/robots/hopper.py @@ -13,22 +13,22 @@ class Hopper(Robot): def __init__(self, simulator, - init_pos=(-0.5, 0, 0.1), - init_orient=(0, 0.707, 0, 0.707), - useFixedBase=False, + position=(-0.5, 0, 0.1), + orientation=(0, 0.707, 0, 0.707), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/mjcfs/hopper.xml'): + urdf=os.path.dirname(__file__) + '/mjcfs/hopper.xml'): # check parameters - if init_pos is None: - init_pos = (-0.5, 0., 0.1) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.1,) - if init_orient is None: - init_orient = (0, 0.707, 0, 0.707) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (-0.5, 0., 0.1) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.1,) + if orientation is None: + orientation = (0, 0.707, 0, 0.707) + if fixed_base is None: + fixed_base = False - super(Hopper, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Hopper, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'hopper' @@ -48,7 +48,7 @@ if __name__ == "__main__": robot = Hopper(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # run simulation for i in count(): diff --git a/pyrobolearn/robots/hubo.py b/pyrobolearn/robots/hubo.py index 8cc3426..485c6ed 100644 --- a/pyrobolearn/robots/hubo.py +++ b/pyrobolearn/robots/hubo.py @@ -27,42 +27,42 @@ class Hubo(BipedRobot, BiManipulatorRobot, TwoHand): def __init__(self, simulator, - init_pos=(0, 0, 1), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 1), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/hubo/hubo.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/hubo/hubo.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 1.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (1.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 1.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (1.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Hubo, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Hubo, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'hubo' - self.neck = [self.getLinkIds(link) for link in ['Body_Neck', 'Body_Head_Empty', 'Body_Head'] + self.neck = [self.get_link_ids(link) for link in ['Body_Neck', 'Body_Head_Empty', 'Body_Head'] if link in self.link_names] - self.head = self.getLinkIds('Body_Head') if 'Body_Head' in self.link_names else None - self.waist = self.getLinkIds('Body_Hip') if 'Body_Hip' in self.link_names else None + self.head = self.get_link_ids('Body_Head') if 'Body_Head' in self.link_names else None + self.waist = self.get_link_ids('Body_Hip') if 'Body_Hip' in self.link_names else None - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['Body_LHY', 'Body_LHR', 'Body_LHP', 'Body_LKN', 'Body_LAP', 'Body_LAR'], ['Body_RHY', 'Body_RHR', 'Body_RHP', 'Body_RKN', 'Body_RAP', 'Body_RAR']]] - self.feet = [self.getLinkIds(link) for link in ['Body_LAR', 'Body_RAR'] if link in self.link_names] + self.feet = [self.get_link_ids(link) for link in ['Body_LAR', 'Body_RAR'] if link in self.link_names] - self.arms = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.arms = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['Body_LSP', 'Body_LSR', 'Body_LSY', 'Body_LEB', 'Body_LWY', 'Body_LWP'], ['Body_RSP', 'Body_RSR', 'Body_RSY', 'Body_REB', 'Body_RWY', 'Body_RWP']]] - self.hands = [self.getLinkIds(link) for link in ['Body_LWP', 'Body_RWP'] if link in self.link_names] + self.hands = [self.get_link_ids(link) for link in ['Body_LWP', 'Body_RWP'] if link in self.link_names] - self.fingers = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.fingers = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['leftThumbProximal', 'leftThumbMedial', 'leftThumbDistal'], ['leftIndexProximal', 'leftIndexMedial', 'leftIndexDistal'], ['leftMiddleProximal', 'leftMiddleMedial', 'leftMiddleDistal'], @@ -94,12 +94,12 @@ if __name__ == "__main__": robot = Hubo(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/humanoid.py b/pyrobolearn/robots/humanoid.py index ce9de86..0e0ac2b 100644 --- a/pyrobolearn/robots/humanoid.py +++ b/pyrobolearn/robots/humanoid.py @@ -14,38 +14,38 @@ class Humanoid(BipedRobot, BiManipulatorRobot): def __init__(self, simulator, - init_pos=(-0.5, 0, 1.), - init_orient=(0, 0.707, 0, 0.707), - useFixedBase=False, + position=(-0.5, 0, 1.), + orientation=(0, 0.707, 0, 0.707), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/mjcfs/humanoid.xml'): # humanoid_symmetric.xml + urdf=os.path.dirname(__file__) + '/mjcfs/humanoid.xml'): # humanoid_symmetric.xml # check parameters - if init_pos is None: - init_pos = (-0.5, 0., 1.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (1.,) - if init_orient is None: - init_orient = (0, 0.707, 0, 0.707) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (-0.5, 0., 1.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (1.,) + if orientation is None: + orientation = (0, 0.707, 0, 0.707) + if fixed_base is None: + fixed_base = False - super(Humanoid, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Humanoid, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'humanoid' - # self.waist = self.getLinkIds('DWL') if 'DWL' in self.link_names else None - # self.torso = self.getLinkIds('DWYTorso') if 'DWYTorso' in self.link_names else None + # self.waist = self.get_link_ids('DWL') if 'DWL' in self.link_names else None + # self.torso = self.get_link_ids('DWYTorso') if 'DWYTorso' in self.link_names else None # - # self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + # self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] # for links in [['LHipMot', 'LThighUpLeg', 'LThighLowLeg', 'LLowLeg', 'LFootmot', 'LFoot'], # ['RHipMot', 'RThighUpLeg', 'RThighLowLeg', 'RLowLeg', 'RFootmot', 'RFoot']]] - self.feet = [self.getLinkIds(link) for link in ['left_foot', 'right_foot'] if link in self.link_names] + self.feet = [self.get_link_ids(link) for link in ['left_foot', 'right_foot'] if link in self.link_names] - # self.arms = [[self.getLinkIds(link) for link in links if link in self.link_names] + # self.arms = [[self.get_link_ids(link) for link in links if link in self.link_names] # for links in [['LShp', 'LShr', 'LShy', 'LElb', 'LForearm', 'LWrMot2', 'LWrMot3'], # ['RShp', 'RShr', 'RShy', 'RElb', 'RForearm', 'RWrMot2', 'RWrMot3']]] - self.hands = [self.getLinkIds(link) for link in ['left_lower_arm', 'right_lower_arm'] + self.hands = [self.get_link_ids(link) for link in ['left_lower_arm', 'right_lower_arm'] if link in self.link_names] @@ -65,13 +65,13 @@ if __name__ == "__main__": robot = Humanoid(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulation for i in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() # step in simulation world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/husky.py b/pyrobolearn/robots/husky.py index cfac8a9..9a1ba6f 100644 --- a/pyrobolearn/robots/husky.py +++ b/pyrobolearn/robots/husky.py @@ -21,25 +21,25 @@ class Husky(DifferentialWheeledRobot): def __init__(self, simulator, - init_pos=(0, 0, .14), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, .14), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/husky/husky.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/husky/husky.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.14) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.14,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.14) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.14,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Husky, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Husky, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'husky' - self.wheels = [self.getLinkIds(link) for link in ['front_left_wheel_link', 'front_right_wheel_link', + self.wheels = [self.get_link_ids(link) for link in ['front_left_wheel_link', 'front_right_wheel_link', 'rear_left_wheel_link', 'rear_right_wheel_link'] if link in self.link_names] self.wheel_directions = np.ones(len(self.wheels)) @@ -61,13 +61,13 @@ if __name__ == "__main__": robot = Husky(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - robot.driveForward(2) + # robot.update_joint_slider() + robot.drive_forward(2) world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/hyq.py b/pyrobolearn/robots/hyq.py index ca02720..63a2ac4 100644 --- a/pyrobolearn/robots/hyq.py +++ b/pyrobolearn/robots/hyq.py @@ -18,31 +18,31 @@ class HyQ(QuadrupedRobot): def __init__(self, simulator, - init_pos=(0, 0, .9), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, .9), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/hyq/hyq.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/hyq/hyq.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.9) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.9,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.9) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.9,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(HyQ, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(HyQ, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'hyq' - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['lf_hipassembly', 'lf_upperleg', 'lf_lowerleg'], ['rf_hipassembly', 'rf_upperleg', 'rf_lowerleg'], ['lh_hipassembly', 'lh_upperleg', 'lh_lowerleg'], ['rh_hipassembly', 'rh_upperleg', 'rh_lowerleg']]] - self.feet = [self.getLinkIds(link) for link in ['lf_foot', 'rf_foot', 'lh_foot', 'rh_foot'] + self.feet = [self.get_link_ids(link) for link in ['lf_foot', 'rf_foot', 'lh_foot', 'rh_foot'] if link in self.link_names] @@ -62,14 +62,14 @@ if __name__ == "__main__": robot = HyQ(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # # Position control using sliders - robot.addJointSlider(robot.getLeftFrontLegIds()) + robot.add_joint_slider(robot.getLeftFrontLegIds()) # run simulator for _ in count(): - robot.updateJointSlider() - robot.computeAndDrawCoMPosition() - robot.computeAndDrawProjectedCoMPosition() + robot.update_joint_slider() + robot.compute_and_draw_com_position() + robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/hyq2max.py b/pyrobolearn/robots/hyq2max.py index 0fbc4dd..9159ee0 100644 --- a/pyrobolearn/robots/hyq2max.py +++ b/pyrobolearn/robots/hyq2max.py @@ -19,37 +19,37 @@ class HyQ2Max(QuadrupedRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.8), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.8), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/hyq2max/hyq2max.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/hyq2max/hyq2max.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.8) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.8,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.8) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.8,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(HyQ2Max, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(HyQ2Max, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'hyq2max' self.height = 0.9 - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['lf_hipassembly', 'lf_upperleg', 'lf_lowerleg'], ['rf_hipassembly', 'rf_upperleg', 'rf_lowerleg'], ['lh_hipassembly', 'lh_upperleg', 'lh_lowerleg'], ['rh_hipassembly', 'rh_upperleg', 'rh_lowerleg']]] - self.feet = [self.getLinkIds(link) for link in ['lf_foot', 'rf_foot', 'lh_foot', 'rh_foot'] + self.feet = [self.get_link_ids(link) for link in ['lf_foot', 'rf_foot', 'lh_foot', 'rh_foot'] if link in self.link_names] for foot in self.feet: - self.sim.changeDynamics(self.id, foot, lateralFriction=.9, spinningFriction=1., rollingFriction=1.) - self.sim.changeDynamics(self.id, foot, restitution=0.) + self.sim.change_dynamics(self.id, foot, lateral_friction=.9, spinning_friction=1., rolling_friction=1.) + self.sim.change_dynamics(self.id, foot, restitution=0.) # taken from "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019 self.kp = 50. * np.ones(12) @@ -67,21 +67,21 @@ if __name__ == "__main__": # create world world = BasicWorld(sim) - world.loadJapaneseMonastery() + world.load_japanese_monastery() # create robot robot = HyQ2Max(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider(robot.getLeftFrontLegIds()) + # robot.add_joint_slider(robot.getLeftFrontLegIds()) # run simulator for _ in count(): - # robot.updateJointSlider() - robot.computeAndDrawCoMPosition() - robot.computeAndDrawProjectedCoMPosition() + # robot.update_joint_slider() + robot.compute_and_draw_com_position() + robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/icub.py b/pyrobolearn/robots/icub.py index 876b3c4..c98b6f1 100644 --- a/pyrobolearn/robots/icub.py +++ b/pyrobolearn/robots/icub.py @@ -16,41 +16,41 @@ class ICub(BipedRobot, BiManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.7), - init_orient=(0, 0, 1, 0), - useFixedBase=False, + position=(0, 0, 0.7), + orientation=(0, 0, 1, 0), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/icub/icub-v2.5+.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/icub/icub-v2.5+.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.7) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.7,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.7) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.7,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(ICub, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase) + super(ICub, self).__init__(simulator, urdf, position, orientation, fixed_base) self.name = 'icub' - self.head = self.getLinkIds('head') if 'head' in self.link_names else None - self.neck = [self.getLinkIds(link) for link in ['neck_1', 'neck_2'] if link in self.link_names] - self.torso = [self.getLinkIds(link) for link in ['torso_1', 'torso_2', 'chest'] if link in self.link_names] + self.head = self.get_link_ids('head') if 'head' in self.link_names else None + self.neck = [self.get_link_ids(link) for link in ['neck_1', 'neck_2'] if link in self.link_names] + self.torso = [self.get_link_ids(link) for link in ['torso_1', 'torso_2', 'chest'] if link in self.link_names] - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['l_hip_1', 'l_hip_2', 'l_upper_leg', 'l_lower_leg', 'l_ankle_1', 'l_ankle_2'], ['r_hip_1', 'r_hip_2', 'r_upper_leg', 'r_lower_leg', 'r_ankle_1', 'r_ankle_2']]] - self.feet = [self.getLinkIds(link) for link in ['l_sole', 'r_sole'] if link in self.link_names] + self.feet = [self.get_link_ids(link) for link in ['l_sole', 'r_sole'] if link in self.link_names] - self.arms = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.arms = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['l_shoulder_1', 'l_shoulder_2', 'l_shoulder_3', 'l_elbow_1', 'l_forearm', 'l_wrist_1', 'l_hand'], ['r_shoulder_1', 'r_shoulder_2', 'r_shoulder_3', 'r_elbow_1', 'r_forearm', 'r_wrist_1', 'r_hand']]] - self.hands = [self.getLinkIds(link) for link in ['l_hand', 'r_hand'] if link in self.link_names] + self.hands = [self.get_link_ids(link) for link in ['l_hand', 'r_hand'] if link in self.link_names] # Test @@ -69,12 +69,12 @@ if __name__ == "__main__": robot = ICub(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/jaco.py b/pyrobolearn/robots/jaco.py index e32b6f8..9e2ef07 100644 --- a/pyrobolearn/robots/jaco.py +++ b/pyrobolearn/robots/jaco.py @@ -18,22 +18,22 @@ class Jaco(ManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), - useFixedBase=True, + position=(0, 0, 0), + orientation=(0, 0, 0, 1), + fixed_base=True, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/jaco/jaco.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/jaco/jaco.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True - super(Jaco, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Jaco, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'jaco' @@ -53,7 +53,7 @@ if __name__ == "__main__": robot = Jaco(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # run simulation for i in count(): diff --git a/pyrobolearn/robots/kr5.py b/pyrobolearn/robots/kr5.py index c45d6aa..2797cfe 100644 --- a/pyrobolearn/robots/kr5.py +++ b/pyrobolearn/robots/kr5.py @@ -20,22 +20,22 @@ class KR5(ManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), - useFixedBase=True, + position=(0, 0, 0), + orientation=(0, 0, 0, 1), + fixed_base=True, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/kuka/kr5/kr5.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/kuka/kr5/kr5.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True - super(KR5, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(KR5, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'kr5' @@ -55,8 +55,8 @@ if __name__ == "__main__": robot = KR5(sim) # print information about the robot - robot.printRobotInfo() - # H = robot.calculateMassMatrix() + robot.print_info() + # H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) for i in count(): diff --git a/pyrobolearn/robots/kuka_iiwa.py b/pyrobolearn/robots/kuka_iiwa.py index 34ef55d..43c612d 100644 --- a/pyrobolearn/robots/kuka_iiwa.py +++ b/pyrobolearn/robots/kuka_iiwa.py @@ -21,25 +21,25 @@ class KukaIIWA(ManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), + position=(0, 0, 0), + orientation=(0, 0, 0, 1), scaling=1., - useFixedBase=True, - urdf_path=os.path.dirname(__file__) + '/urdfs/kuka/kuka_iiwa/iiwa14.urdf'): + fixed_base=True, + urdf=os.path.dirname(__file__) + '/urdfs/kuka/kuka_iiwa/iiwa14.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True - super(KukaIIWA, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(KukaIIWA, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'kuka_iiwa' - # self.disableMotor() + # self.disable_motor() # Test @@ -59,11 +59,11 @@ if __name__ == "__main__": robot = KukaIIWA(sim) # print information about the robot - robot.printRobotInfo() - # H = robot.calculateMassMatrix() + robot.print_info() + # H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) - # print(robot.getLinkWorldPositions(flatten=False)) + # print(robot.get_link_world_positions(flatten=False)) K = 5000*np.identity(3) # D = 2 * np.sqrt(K) @@ -71,22 +71,22 @@ if __name__ == "__main__": D = 100 * np.identity(3) x_des = np.array([0.3, 0.0, 0.8]) x_des = np.array([0.52557296, 0.09732758, 0.80817658]) - linkId = robot.getLinkIds('iiwa_link_ee') + linkId = robot.get_link_ids('iiwa_link_ee') for i in count(): - # print(robot.getLinkWorldPositions(flatten=False)) + # print(robot.get_link_world_positions(flatten=False)) # get state - q = robot.getJointPositions() - dq = robot.getJointVelocities() - x = robot.getLinkWorldPositions(linkId) - dx = robot.getLinkWorldLinearVelocities(linkId) + q = robot.get_joint_positions() + dq = robot.get_joint_velocities() + x = robot.get_link_world_positions(linkId) + dx = robot.get_link_world_linear_velocities(linkId) # get (linear) jacobian J = robot.getLinearJacobian(linkId, q) # get coriolis, gravity compensation torques - torques = robot.getCoriolisAndGravityCompensationTorques(q, dq) + torques = robot.get_coriolis_and_gravity_compensation_torques(q, dq) # Impedance control: attractor point F = K.dot(x_des - x) - D.dot(dx) @@ -94,7 +94,7 @@ if __name__ == "__main__": tau = J.T.dot(F) print(tau) torques += tau - robot.setJointTorques(torques) + robot.set_joint_torques(torques) # step in simulation world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/kuka_lwr.py b/pyrobolearn/robots/kuka_lwr.py index 2e1bd48..bad62bf 100644 --- a/pyrobolearn/robots/kuka_lwr.py +++ b/pyrobolearn/robots/kuka_lwr.py @@ -21,22 +21,22 @@ class KukaLWR(ManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), - useFixedBase=True, + position=(0, 0, 0), + orientation=(0, 0, 0, 1), + fixed_base=True, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/kuka/kuka_lwr/kuka.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/kuka/kuka_lwr/kuka.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True - super(KukaLWR, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(KukaLWR, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'kuka_lwr' @@ -56,8 +56,8 @@ if __name__ == "__main__": robot = KukaLWR(sim) # print information about the robot - robot.printRobotInfo() - # H = robot.calculateMassMatrix() + robot.print_info() + # H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) for i in count(): diff --git a/pyrobolearn/robots/laikago.py b/pyrobolearn/robots/laikago.py index 13e053d..c2f7112 100644 --- a/pyrobolearn/robots/laikago.py +++ b/pyrobolearn/robots/laikago.py @@ -18,36 +18,36 @@ class Laikago(QuadrupedRobot): def __init__(self, simulator, - init_pos=(0, 0, .5), - init_orient=(0.5, 0.5, 0.5, 0.5), - useFixedBase=False, + position=(0, 0, .5), + orientation=(0.5, 0.5, 0.5, 0.5), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/laikago/laikago.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/laikago/laikago.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.5) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.5,) - if init_orient is None: - init_orient = (0.5, 0.5, 0.5, 0.5) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.5) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.5,) + if orientation is None: + orientation = (0.5, 0.5, 0.5, 0.5) + if fixed_base is None: + fixed_base = False - super(Laikago, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Laikago, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'laikago' - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['FL_hip_motor', 'FL_upper_leg', 'FL_lower_leg'], ['FR_hip_motor', 'FR_upper_leg', 'FR_lower_leg'], ['RL_hip_motor', 'RL_upper_leg', 'RL_lower_leg'], ['RR_hip_motor','RR_upper_leg', 'RR_lower_leg']]] - self.feet = [self.getLinkIds(link) for link in ['FL_lower_leg', 'FR_lower_leg', + self.feet = [self.get_link_ids(link) for link in ['FL_lower_leg', 'FR_lower_leg', 'RL_lower_leg', 'RR_lower_leg'] if link in self.link_names] - def getHomeJointPositions(self): + def get_home_joint_positions(self): """Return the joint positions for the home position""" - return np.zeros(self.getNumberOfDoFs()) + return np.zeros(self.num_dofs) # Test @@ -66,13 +66,13 @@ if __name__ == "__main__": robot = Laikago(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - robot.moveJointHomePositions() + # robot.update_joint_slider() + robot.move_joint_home_positions() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/legged_robot.py b/pyrobolearn/robots/legged_robot.py index dbab864..23e051f 100644 --- a/pyrobolearn/robots/legged_robot.py +++ b/pyrobolearn/robots/legged_robot.py @@ -1,5 +1,7 @@ #!/usr/bin/env python """Provide the Legged robot abstract classes. + +Classes that are defined here: LeggedRobot, BipedRobot, QuadrupedRobot, HexapodRobot. """ import collections @@ -15,14 +17,8 @@ class LeggedRobot(Robot): in the standard regime are rhythmic movements. """ - def __init__(self, - simulator, - urdf_path, - init_pos=(0, 0, 1.), - init_orient=(0, 0, 0, 1), - useFixedBase=False, - scaling=1.): - super(LeggedRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling=1.) + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.): + super(LeggedRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling=scaling) self.legs = [] # list of legs where a leg is a list of links self.feet = [] # list of feet ids @@ -45,75 +41,120 @@ class LeggedRobot(Robot): # Methods # ########### - def getNumberOfLegs(self): + def get_leg_ids(self, legs=None): """ - Return the number of legs/feet. + Return the leg id associated with the given leg index(ices)/name(s). + + Args: + legs (int, str): leg index(ices) which is [0..num_legs()], or leg name(s) Returns: - int: the number of legs/feet + int, list[int]: leg id(s) """ - return self.num_legs + if legs is not None: + if isinstance(legs, int): + return self.legs[legs] + elif isinstance(legs, str): + return self.legs[self.get_link_ids(legs)] + elif isinstance(legs, (list, tuple)): + leg_ids = [] + for leg in legs: + if isinstance(leg, int): + leg_ids.append(self.legs[leg]) + elif isinstance(leg, str): + leg_ids.append(self.legs[self.get_link_ids(leg)]) + else: + raise TypeError("Expecting a str or int for items in legs") + return leg_ids + return self.legs - def getLegLinkIds(self, legIds=None): - pass + def get_feet_ids(self, feet=None): + """ + Return the foot id associated with the given foot index(ices)/name(s). - def getLegLinkNames(self, legIds=None): - pass + Args: + feet (int, str): foot index(ices) which is [0..num_feet()], or foot name(s) - def getFeetIds(self, footId=None): - pass + Returns: + int, list[int]: foot id(s) + """ + if feet is not None: + if isinstance(feet, int): + return self.feet[feet] + elif isinstance(feet, str): + return self.feet[self.get_link_ids(feet)] + elif isinstance(feet, (list, tuple)): + foot_ids = [] + for foot in feet: + if isinstance(foot, int): + foot_ids.append(self.feet[foot]) + elif isinstance(foot, str): + foot_ids.append(self.feet[self.get_link_ids(foot)]) + else: + raise TypeError("Expecting a str or int for items in feet") + return foot_ids + return self.feet - def getFeetNames(self, footIds=None): - pass + def set_foot_friction(self, frictions, feet_ids=None): + """ + Set the foot friction in the simulator. - def getCoP(self): + Warnings: only available in the simulator. + + Args: + frictions (float, list of float): friction value(s). + feet_ids (int, list of int): list of foot/feet id(s). + """ + if feet_ids is None: + feet_ids = self.feet + if isinstance(feet_ids, int): + feet_ids = [feet_ids] + if isinstance(frictions, (float, int)): + frictions = frictions * np.ones(len(feet_ids)) + for foot_id, frict in zip(feet_ids, frictions): + if isinstance(foot_id, int): + self.sim.change_dynamics(self.id, foot_id, lateral_friction=frict) + elif isinstance(foot_id, collections.Iterable): + for idx in foot_id: + self.sim.change_dynamics(self.id, idx, lateral_friction=frict) + else: + raise TypeError("Expecting foot_id to be a list of int, or an int. Instead got: " + "{}".format(type(foot_id))) + + def center_of_pressure(self): """ Center of Pressure - - Returns: - """ # self.sim.getContactPoints(self.id, FootID) # use simulator # use F/T sensor to get CoP pass - def getZMP(self): + def zero_moment_point(self): """ Zero Moment Point. Assumptions: the contact area is planar and has sufficiently high friction to keep the feet from sliding. - - Returns: - """ pass - def getFRI(self): + def foot_rotation_index(self): """ Foot Rotation Index - - Returns: - """ pass - def getDCM(self): + def divergent_component_motion(self): """ Divergent Component of Motion, a.k.a 'eXtrapolated Center of Mass' - - Returns: - """ pass - def getCMP(self): + def centroidal_moment_pivot(self): """ Centroidal Moment Pivot - - Returns: - """ + pass - def drawSupportPolygon(self): + def draw_support_polygon(self): """ draw the support polygon / convex hull """ @@ -122,54 +163,34 @@ class LeggedRobot(Robot): # the following methods need to be overwritten in the children classes def move(self, velocity): - raise NotImplementedError + """Move the robot at the specified velocity.""" + pass - def walkForward(self): - raise NotImplementedError + def walk_forward(self): + """Walk forward.""" + pass - def walkBackward(self): - raise NotImplementedError + def walk_backward(self): + """Walk backward.""" + pass - def turnLeft(self): - raise NotImplementedError + def turn_left(self): + """Turn left.""" + pass - def turnRight(self): - raise NotImplementedError - - def setFootFriction(self, friction, feet_id=None): - """ - Set the foot friction in the simulator. - - Warnings: only available in the simulator. - - Args: - friction (float, list of float): friction value(s). - feet_id (int, list of int): list of foot/feet id(s). - """ - if feet_id is None: - foot_id = self.feet - if isinstance(feet_id, int): - feet_id = [feet_id] - if isinstance(friction, (float, int)): - friction = friction * np.ones(len(feet_id)) - for foot_id, frict in zip(feet_id, friction): - if isinstance(foot_id, int): - self.sim.changeDynamics(self.id, foot_id, lateralFriction=frict) - elif isinstance(foot_id, collections.Iterable): - for idx in foot_id: - self.sim.changeDynamics(self.id, idx, lateralFriction=frict) - else: - raise TypeError("Expecting foot_id to be a list of int, or an int. Instead got: " - "{}".format(type(foot_id))) + def turn_right(self): + """Turn right.""" + pass class BipedRobot(LeggedRobot): r"""Biped Robot + A biped robot is a robot which has 2 legs. """ - def __init__(self, simulator, urdf_path, init_pos=(0,0,1.5), init_orient=(0,0,0,1), useFixedBase=False, scaling=1.): - super(BipedRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.): + super(BipedRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.left_leg_id = 0 self.right_leg_id = 1 @@ -180,48 +201,33 @@ class BipedRobot(LeggedRobot): @property def left_leg(self): + """Return the left leg joint ids""" return self.legs[self.left_leg_id] @property def right_leg(self): + """Return the right leg joint ids""" return self.legs[self.right_leg_id] - ########### - # Methods # - ########### - - def getLeftLegIds(self): - """ - Return the left leg actuated joint/link ids. - """ - return self.legs[self.left_leg_id] - - def getRightLegIds(self): - """ - Return the right leg actuated joint/link ids. - """ - return self.legs[self.right_leg_id] - - def getLeftFootId(self): - """ - Return the left foot id. - """ + @property + def left_foot(self): + """Return the left foot id""" return self.feet[self.left_leg_id] - def getRightFootId(self): - """ - Return the right foot id. - """ + @property + def right_foot(self): + """Return the right foot id""" return self.feet[self.right_leg_id] class QuadrupedRobot(LeggedRobot): r"""Quadruped robot + A quadruped robot is a robot which has 4 legs. """ - def __init__(self, simulator, urdf_path, init_pos=(0,0,1.), init_orient=(0,0,0,1), useFixedBase=False, scaling=1.): - super(QuadrupedRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.): + super(QuadrupedRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.left_front_leg_id = 0 self.right_front_leg_id = 1 @@ -234,61 +240,53 @@ class QuadrupedRobot(LeggedRobot): @property def left_front_leg(self): + """Return the left front leg joint ids""" return self.legs[self.left_front_leg_id] @property def right_front_leg(self): + """Return the right front leg joint ids""" return self.legs[self.right_front_leg_id] @property def left_back_leg(self): + """Return the left back leg joint ids""" return self.legs[self.left_back_leg_id] @property def right_back_leg(self): + """Return the right back leg joint ids""" return self.legs[self.right_back_leg_id] - ########### - # Methods # - ########### - - def getLeftFrontLegIds(self): - """Return the left front leg ids""" - return self.legs[self.left_front_leg_id] - - def getLeftFrontFootId(self): + @property + def left_front_foot(self): """Return the left front foot id""" return self.feet[self.left_front_leg_id] - def getRightFrontLegIds(self): - """Return the right front leg ids""" - return self.legs[self.right_front_leg_id] - - def getRightFrontFootId(self): + @property + def right_front_foot(self): """Return the right front foot id""" return self.feet[self.right_front_leg_id] - def getLeftBackLegIds(self): - """Return the left back leg ids""" - return self.legs[self.left_back_leg_id] - - def getLeftBackFootId(self): + @property + def left_back_foot(self): """Return the left back foot id""" return self.feet[self.left_back_leg_id] - def getRightBackLegIds(self): - """Return the right back leg ids""" - return self.legs[self.right_back_leg_id] - - def getRightBackFootId(self): + @property + def right_back_foot(self): """Return the right back foot id""" return self.feet[self.right_back_leg_id] class HexapodRobot(LeggedRobot): + r"""Hexapod Robot - def __init__(self, simulator, urdf_path, init_pos=(0,0,1.), init_orient=(0,0,0,1), useFixedBase=False, scaling=1.): - super(HexapodRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + An hexapod robot is a robot which has 6 legs. + """ + + def __init__(self, simulator, urdf, position, orientation=None, fixed_base=False, scaling=1.): + super(HexapodRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.left_front_leg_id = 0 self.right_front_leg_id = 1 @@ -303,76 +301,60 @@ class HexapodRobot(LeggedRobot): @property def left_front_leg(self): + """Return the left front leg ids""" return self.legs[self.left_front_leg_id] @property def right_front_leg(self): + """Return the right front leg ids""" return self.legs[self.right_front_leg_id] @property def left_middle_leg(self): + """Return the left middle leg ids""" return self.legs[self.left_middle_leg_id] @property def right_middle_leg(self): + """Return the right middle leg ids""" return self.legs[self.right_middle_leg_id] @property def left_back_leg(self): + """Return the left back leg ids""" return self.legs[self.left_back_leg_id] @property def right_back_leg(self): - return self.legs[self.right_back_leg_id] - - ########### - # Methods # - ########### - - def getLeftFrontLegIds(self): - """Return the left front leg ids""" - return self.legs[self.left_front_leg_id] - - def getLeftFrontFootId(self): - """Return the left front foot id""" - return self.feet[self.left_front_leg_id] - - def getRightFrontLegIds(self): - """Return the right front leg ids""" - return self.legs[self.right_front_leg_id] - - def getRightFrontFootId(self): - """Return the right front foot id""" - return self.feet[self.right_front_leg_id] - - def getLeftMiddleLegIds(self): - """Return the left middle leg ids""" - return self.legs[self.left_middle_leg_id] - - def getLeftMiddleFootId(self): - """Return the left middle foot id""" - return self.feet[self.left_middle_leg_id] - - def getRightMiddleLegIds(self): - """Return the right middle leg ids""" - return self.legs[self.right_middle_leg_id] - - def getRightMiddleFootId(self): - """Return the right middle foot id""" - return self.feet[self.right_middle_leg_id] - - def getLeftBackLegIds(self): - """Return the left back leg ids""" - return self.legs[self.left_back_leg_id] - - def getLeftBackFootId(self): - """Return the left back foot id""" - return self.feet[self.left_back_leg_id] - - def getRightBackLegIds(self): """Return the right back leg ids""" return self.legs[self.right_back_leg_id] - def getRightBackFootId(self): + @property + def left_front_foot(self): + """Return the left front foot id""" + return self.feet[self.left_front_leg_id] + + @property + def right_front_foot(self): + """Return the right front foot id""" + return self.feet[self.right_front_leg_id] + + @property + def left_middle_foot(self): + """Return the left middle foot id""" + return self.feet[self.left_middle_leg_id] + + @property + def right_middle_foot(self): + """Return the right middle foot id""" + return self.feet[self.right_middle_leg_id] + + @property + def left_back_foot(self): + """Return the left back foot id""" + return self.feet[self.left_back_leg_id] + + @property + def right_back_foot(self): """Return the right back foot id""" return self.feet[self.right_back_leg_id] diff --git a/pyrobolearn/robots/littledog.py b/pyrobolearn/robots/littledog.py index 769d8d7..5e9a0a4 100644 --- a/pyrobolearn/robots/littledog.py +++ b/pyrobolearn/robots/littledog.py @@ -15,38 +15,38 @@ class LittleDog(QuadrupedRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.2), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.2), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/littledog/littleDog.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/littledog/littleDog.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.2) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.2,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.2) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.2,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(LittleDog, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(LittleDog, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'littledog' - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['front_left_hip', 'front_left_upper_leg', 'front_left_lower_leg'], ['front_right_hip', 'front_right_upper_leg', 'front_right_lower_leg'], ['back_left_hip', 'back_left_upper_leg', 'back_left_lower_leg'], ['back_right_hip', 'back_right_upper_leg', 'back_right_lower_leg']]] - self.feet = [self.getLinkIds(link) for link in ['front_left_lower_leg', 'front_right_lower_leg', + self.feet = [self.get_link_ids(link) for link in ['front_left_lower_leg', 'front_right_lower_leg', 'back_left_lower_leg', 'back_right_lower_leg'] if link in self.link_names] self.kp = 24. * np.ones(12) self.kd = np.array([0.5, 0.5, 0.16, 0.5, 0.5, 0.16, 0.5, 0.5, 0.16, 0.5, 0.5, 0.16]) - self.setJointPositions([-0.6, -0.6, 0.6, 0.6], self.feet) + self.set_joint_positions([-0.6, -0.6, 0.6, 0.6], self.feet) self.joint_nominal_config = np.array([0., 0., -0.6, 0., 0., -0.6, 0., 0., 0.6, 0., 0., 0.6]) @@ -67,14 +67,14 @@ if __name__ == "__main__": robot = LittleDog(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - # robot.computeAndDrawCoMPosition() - # robot.computeAndDrawProjectedCoMPosition() + # robot.update_joint_slider() + # robot.compute_and_draw_com_position() + # robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/manipulator.py b/pyrobolearn/robots/manipulator.py index 8932a5f..0d8a21d 100644 --- a/pyrobolearn/robots/manipulator.py +++ b/pyrobolearn/robots/manipulator.py @@ -13,12 +13,12 @@ class ManipulatorRobot(Robot): def __init__(self, simulator, - urdf_path, - init_pos=(0, 0, 0.), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + urdf, + position=(0, 0, 0.), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1.): - super(ManipulatorRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(ManipulatorRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.arms = [] # list of arms where an arm is a list of links self.hands = [] # list of end-effectors/hands @@ -41,24 +41,12 @@ class ManipulatorRobot(Robot): # Methods # ########### - def getNumberOfArms(self): - """ - Return the number of arms/hands. - - Returns: - int: the number of arms/hands - """ - return self.num_arms - - # alias (normally this is correct) - getNumberOfHands = getNumberOfArms - - def getArmLinkIds(self, armLink=None): + def get_arm_ids(self, arms=None): """ Return the arm's link id(s) from the name(s) or index(ices). Args: - armLink (str, int, list of str/int, None): if str, it will get the arm's link id associated to the given + arms (str, int, list of str/int, None): if str, it will get the arm's link id associated to the given name. If int, it will get the arm's link id associated to the given index. If it is a list of str and/or int, it will get the corresponding arm's link ids. If None, it will return all the arm's link ids. @@ -69,49 +57,55 @@ class ManipulatorRobot(Robot): if multiple arm's links: int[N]: link ids """ - if armLink is None: - return self.arms + if arms is not None: + if isinstance(arms, int): + return self.arms[arms] + elif isinstance(arms, str): + return self.arms[self.get_link_ids(arms)] + elif isinstance(arms, (list, tuple)): + arm_ids = [] + for arm in arms: + if isinstance(arm, int): + arm_ids.append(self.arms[arm]) + elif isinstance(arm, str): + arm_ids.append(self.arms[self.get_link_ids(arm)]) + else: + raise TypeError("Expecting a str or int for items in arms") + return arm_ids + return self.arms - def getIndex(link): - if isinstance(link, str): - return self.arm_names[link] - elif isinstance(link, int): - return self.arms[link] - else: - raise TypeError("Expecting an int or str.") - - # list of links in the arm - if isinstance(armLink, collections.Iterable) and not isinstance(armLink, str): - return [getIndex(link) for link in armLink] - - # one link in the arm - return getIndex(armLink) - - def getArmLinkNames(self, armLinkId=None): + def get_hand_ids(self, hands=None): """ - Return the name of the given arm's link(s). + Return the hand's link id(s) from the name(s) or index(ices). Args: - armLinkId (int, int[N], None): link id, or list of desired link ids. If None, get the name of all links - in the arms. + hands (str, int, list of str/int, None): if str, it will get the hand's link id associated to the given + name. If int, it will get the hand's link id associated to the given index. If it is a list of str + and/or int, it will get the corresponding hand's link ids. If None, it will return all the hand's + link ids. Returns: - if 1 arm's link: - str: link name - if multiple arm's links: - str[N]: link names + if 1 hand's link: + int: link id + if multiple hand's links: + int[N]: link ids """ - if isinstance(armLinkId, int): - return self.sim.getJointInfo(self.id, armLinkId)[12] - if armLinkId is None: - armLinkId = [link for arm in self.arms for link in arm] - return [self.sim.getJointInfo(self.id, link)[12] for link in armLinkId] - - def getHandIds(self): - pass - - def getHandNames(self): - pass + if hands is not None: + if isinstance(hands, int): + return self.hands[hands] + elif isinstance(hands, str): + return self.hands[self.get_link_ids(hands)] + elif isinstance(hands, (list, tuple)): + hand_ids = [] + for hand in hands: + if isinstance(hand, int): + hand_ids.append(self.hands[hand]) + elif isinstance(hand, str): + hand_ids.append(self.hands[self.get_link_ids(hand)]) + else: + raise TypeError("Expecting a str or int for items in hands") + return hand_ids + return self.hands class BiManipulatorRobot(ManipulatorRobot): @@ -120,9 +114,9 @@ class BiManipulatorRobot(ManipulatorRobot): Bi-manipulators are robots that have two manipulators to manipulate objects in the environment. """ - def __init__(self, simulator, urdf_path, init_pos=(0, 0, 1.5), init_orient=(0, 0, 0, 1), useFixedBase=False, + def __init__(self, simulator, urdf, position=(0, 0, 1.5), orientation=(0, 0, 0, 1), fixed_base=False, scaling=1.): - super(BiManipulatorRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(BiManipulatorRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.left_arm_id = 0 self.left_hand_id = 0 @@ -148,23 +142,3 @@ class BiManipulatorRobot(ManipulatorRobot): @property def right_hand(self): return self.hands[self.right_arm_id] - - ########### - # Methods # - ########### - - def getLeftArmIds(self): - """Return the left arm joint ids""" - return self.arms[self.left_arm_id] - - def getLeftHandId(self): - """Return the left hand id""" - return self.hands[self.left_hand_id] - - def getRightArmIds(self): - """Return the right arm joint ids""" - return self.arms[self.right_arm_id] - - def getRightHandId(self): - """Return the right hand id""" - return self.hands[self.right_hand_id] diff --git a/pyrobolearn/robots/manipulator2d.py b/pyrobolearn/robots/manipulator2d.py index 6d4d1ef..1d2d8ed 100644 --- a/pyrobolearn/robots/manipulator2d.py +++ b/pyrobolearn/robots/manipulator2d.py @@ -17,22 +17,22 @@ class Manipulator2D(ManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/manipulator2d/manipulator2d.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/manipulator2d/manipulator2d.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Manipulator2D, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Manipulator2D, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'manipulator2d' @@ -49,14 +49,14 @@ if __name__ == "__main__": world = BasicWorld(sim) # create robot - robot = Manipulator2D(sim, init_pos=(0, -0.25, 0)) - robot1 = Manipulator2D(sim, init_pos=(0, 0.25, 0)) - robot.printRobotInfo() + robot = Manipulator2D(sim, position=(0, -0.25, 0)) + robot1 = Manipulator2D(sim, position=(0, 0.25, 0)) + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/minitaur.py b/pyrobolearn/robots/minitaur.py index 4956830..c0fac67 100644 --- a/pyrobolearn/robots/minitaur.py +++ b/pyrobolearn/robots/minitaur.py @@ -21,27 +21,28 @@ class Minitaur(QuadrupedRobot): def __init__(self, simulator, - init_pos=(0, 0, .3), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, .3), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., couple_legs=True, foot_friction=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/minitaur/minitaur.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/minitaur/minitaur.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.3) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.3,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.3) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.3,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Minitaur, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Minitaur, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'minitaur' - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.base_height = 0.1638 + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['motor_front_leftL_link', 'lower_leg_front_leftL_link', 'motor_front_leftR_link', 'lower_leg_front_leftR_link'], ['motor_front_rightL_link', 'lower_leg_front_rightL_link', @@ -51,7 +52,7 @@ class Minitaur(QuadrupedRobot): ['motor_back_rightL_link', 'lower_leg_back_rightL_link', 'motor_back_rightR_link', 'lower_leg_back_rightR_link']]] - self.feet = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.feet = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['lower_leg_front_leftL_link', 'lower_leg_front_leftR_link'], ['lower_leg_front_rightL_link', 'lower_leg_front_rightR_link'], ['lower_leg_back_leftL_link', 'lower_leg_back_leftR_link'], @@ -72,20 +73,19 @@ class Minitaur(QuadrupedRobot): self.outer_joints = set([joint for leg in self.outer_legs for joint in leg]) - self.init_joint_positions = self.getHomeJointPositions() + self.init_joint_positions = self.get_home_joint_positions() # constraints self.couple_legs = couple_legs if self.couple_legs: for leg in self.legs: - self.sim.createConstraint(parentBodyUniqueId=self.id, parentLinkIndex=leg[3], - childBodyUniqueId=self.id, childLinkIndex=leg[1], - jointType=self.sim.JOINT_POINT2POINT, - jointAxis=[0, 0, 0], parentFramePosition=[0, 0.005, 0.2], - childFramePosition=[0, 0.01, 0.2]) + self.sim.create_constraint(parent_body_id=self.id, parent_link_id=leg[3], + child_body_id=self.id, child_link_id=leg[1], + joint_type=self.sim.JOINT_POINT2POINT, joint_axis=[0, 0, 0], + parent_frame_position=[0, 0.005, 0.2], child_frame_position=[0, 0.01, 0.2]) # disable motors - self.disableMotor(self.knees) + self.disable_motor(self.knees) # kp, kd gains self.kp = 1. @@ -93,10 +93,10 @@ class Minitaur(QuadrupedRobot): self.max_force = 3.5 # set feet friction - self.setFootFriction(friction=foot_friction, feet_id=self.feet) + self.set_foot_friction(frictions=foot_friction, feet_ids=self.feet) # set joint angles to home position - self.setJointHomePositions() + self.set_joint_home_positions() ############## # Properties # @@ -154,7 +154,7 @@ class Minitaur(QuadrupedRobot): # Methods # ########### - def getHomeJointPositions(self): + def get_home_joint_positions(self): """Return the joint positions for the home position""" h = np.pi/2 # hip angle from [2] k = 2.1834 # knee angle from [2] @@ -166,46 +166,46 @@ class Minitaur(QuadrupedRobot): return np.array(right_front_leg_initial_pos + right_back_leg_initial_pos + left_front_leg_initial_pos + left_back_leg_initial_pos) - def setJointPositions(self, position, jointId=None, kp=None, kd=None, velocity=None, maxTorque=None): + def set_joint_positions(self, positions, joint_ids=None, kp=None, kd=None, velocities=None, forces=None): if self.couple_legs: # assume the joint ids are for the outer legs - if jointId is None: + if joint_ids is None: pass # if the given joint is just one id - elif isinstance(jointId, int): - if jointId not in self.outer_joints: + elif isinstance(joint_ids, int): + if joint_ids not in self.outer_joints: raise ValueError("Expecting the jointId to be an outer joint as the legs of the minitaur are " "coupled") - jointId = [jointId, jointId + 3] - if isinstance(position, collections.Iterable): - position = position[0] + joint_ids = [joint_ids, joint_ids + 3] + if isinstance(positions, collections.Iterable): + positions = positions[0] - position = np.array([position, -position]) - position += self.init_joint_positions[self.getQIndex(jointId)] + positions = np.array([positions, -positions]) + positions += self.init_joint_positions[self.get_q_indices(joint_ids)] # if multiple joint ids - elif isinstance(jointId, collections.Iterable): + elif isinstance(joint_ids, collections.Iterable): # for each outer joint id, get the corresponding inner joint id joints = [] - for joint in jointId: + for joint in joint_ids: if joint not in self.outer_joints: raise ValueError("One of the jointId is not an outer joint which is a problem as the legs of " "the minitaur are coupled") joints.append(joint+3) # increase the list of joint ids to take into account inner joint ids - jointId = list(jointId) + joints + joint_ids = list(joint_ids) + joints # compute the positions (offset original position, and compute positions for inner joints) - position = list(position) + list(-position) - position = np.array(position) + self.init_joint_positions[self.getQIndex(jointId)] + positions = list(positions) + list(-positions) + positions = np.array(positions) + self.init_joint_positions[self.get_q_indices(joint_ids)] else: raise TypeError("Unknown type of for jointId; expecting a list of int, or an int, got instead :" - "{}".format(type(jointId))) - super(Minitaur, self).setJointPositions(position, jointId=jointId, kp=kp, kd=kd, velocity=velocity, - maxTorque=maxTorque) + "{}".format(type(joint_ids))) + super(Minitaur, self).set_joint_positions(positions, joint_ids=joint_ids, kp=kp, kd=kd, velocities=velocities, + forces=forces) # Test @@ -224,20 +224,21 @@ if __name__ == "__main__": robot = Minitaur(sim, couple_legs=True) # print information about the robot - robot.printRobotInfo() - print("Robot leg ids: {}".format(robot.legs)) - print("Robot feet ids: {}".format(robot.feet)) + robot.print_info() + # print("Robot leg ids: {}".format(robot.legs)) + # print("Robot feet ids: {}".format(robot.feet)) # Position control using sliders - # robot.addJointSlider(robot.getLeftFrontLegIds()) + # robot.add_joint_slider(robot.getLeftFrontLegIds()) t = 0 # run simulator for _ in count(): t += 0.01 position = np.pi/4 * np.sin(2 * np.pi * t) * np.ones(len(robot.outer_hips)) - robot.setJointPositions(position, robot.outer_hips) - # robot.updateJointSlider() - # robot.computeAndDrawCoMPosition() - # robot.computeAndDrawProjectedCoMPosition() + # print(robot.get_base_position()) + # robot.set_joint_positions(position, robot.outer_hips) + # robot.update_joint_slider() + # robot.compute_and_draw_com_position() + # robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/mkz.py b/pyrobolearn/robots/mkz.py index fae1a08..3e0411f 100644 --- a/pyrobolearn/robots/mkz.py +++ b/pyrobolearn/robots/mkz.py @@ -21,35 +21,35 @@ class MKZ(AckermannWheeledRobot): def __init__(self, simulator, - init_pos=(0, 0, .4), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, .4), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/mkz/mkz.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/mkz/mkz.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.4) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.4,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.4) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.4,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(MKZ, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(MKZ, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'mkz' - self.wheels = [self.getLinkIds(link) for link in ['wheel_fl', 'wheel_fr', 'wheel_rl', 'wheel_rr'] + self.wheels = [self.get_link_ids(link) for link in ['wheel_fl', 'wheel_fr', 'wheel_rl', 'wheel_rr'] if link in self.link_names] self.wheel_directions = np.ones(len(self.wheels)) - self.steering = [self.getLinkIds(link) for link in ['steer_fl', 'steer_fr'] + self.steering = [self.get_link_ids(link) for link in ['steer_fl', 'steer_fr'] if link in self.link_names] - def setSteering(self, angle): + def set_steering(self, angle): """Set steering angle""" angle = angle * np.ones(len(self.steering)) - self.setJointPositions(angle, jointId=self.steering) + self.set_joint_positions(angle, joint_ids=self.steering) # Test @@ -68,13 +68,13 @@ if __name__ == "__main__": robot = MKZ(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - robot.driveForward(2) + # robot.update_joint_slider() + robot.drive_forward(2) world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/morphex.py b/pyrobolearn/robots/morphex.py index ba7139b..5ee4b16 100644 --- a/pyrobolearn/robots/morphex.py +++ b/pyrobolearn/robots/morphex.py @@ -17,22 +17,22 @@ class Morphex(HexapodRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.2), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.2), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/morphex/morphex.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/morphex/morphex.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.2) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.2,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.2) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.2,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Morphex, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Morphex, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'morphex' @@ -52,7 +52,7 @@ if __name__ == "__main__": robot = Morphex(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # run simulation for i in count(): diff --git a/pyrobolearn/robots/nao.py b/pyrobolearn/robots/nao.py index 6278627..0268d42 100644 --- a/pyrobolearn/robots/nao.py +++ b/pyrobolearn/robots/nao.py @@ -16,40 +16,40 @@ class Nao(BipedRobot, BiManipulatorRobot, TwoHand): def __init__(self, simulator, - init_pos=(0, 0, 0.35), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.35), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/nao/nao_v40.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/nao/nao_v40.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.35) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.35,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.35) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.35,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Nao, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Nao, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'nao' - self.neck = self.getLinkIds('Neck') if 'Neck' in self.link_names else None - self.head = self.getLinkIds('Head') if 'Head' in self.link_names else None + self.neck = self.get_link_ids('Neck') if 'Neck' in self.link_names else None + self.head = self.get_link_ids('Head') if 'Head' in self.link_names else None - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['LPelvis', 'LHip', 'LThigh', 'LTibia', 'LAnklePitch', 'l_ankle'], ['RPelvis', 'RHip', 'RThigh', 'RTibia', 'RAnklePitch', 'r_ankle']]] - self.feet = [self.getLinkIds(link) for link in ['l_sole', 'r_sole'] if link in self.link_names] + self.feet = [self.get_link_ids(link) for link in ['l_sole', 'r_sole'] if link in self.link_names] - self.arms = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.arms = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['LShoulder', 'LBicep', 'LElbow', 'LForeArm', 'l_wrist', 'l_gripper'], ['RShoulder', 'RBicep', 'RElbow', 'RForeArm', 'r_wrist', 'r_gripper']]] - self.hands = [self.getLinkIds(link) for link in ['l_gripper', 'r_gripper'] if link in self.link_names] + self.hands = [self.get_link_ids(link) for link in ['l_gripper', 'r_gripper'] if link in self.link_names] - self.fingers = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.fingers = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['LThumb1_link', 'LThumb2_link'], ['LFinger11_link', 'LFinger12_link', 'LFinger13_link'], ['LFinger21_link', 'LFinger22_link', 'LFinger23_link'], @@ -77,12 +77,12 @@ if __name__ == "__main__": robot = Nao(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider(robot.getLeftArmIds()) + # robot.add_joint_slider(robot.getLeftArmIds()) # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/opendog.py b/pyrobolearn/robots/opendog.py index 06835ec..fee6661 100644 --- a/pyrobolearn/robots/opendog.py +++ b/pyrobolearn/robots/opendog.py @@ -17,31 +17,31 @@ class OpenDog(QuadrupedRobot): def __init__(self, simulator, - init_pos=(0, 0, .6), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, .6), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/opendog/opendog.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/opendog/opendog.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.6) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.6,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.6) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.6,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(OpenDog, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(OpenDog, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'opendog' - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['lf_hip', 'lf_upperleg', 'lf_lowerleg'], ['rf_hip', 'rf_upperleg', 'rf_lowerleg'], ['lb_hip', 'lb_upperleg', 'lb_lowerleg'], ['rb_hip', 'rb_upperleg', 'rb_lowerleg']]] - self.feet = [self.getLinkIds(link) for link in ['lf_lowerleg', 'rf_lowerleg', 'lb_lowerleg', 'rb_lowerleg'] + self.feet = [self.get_link_ids(link) for link in ['lf_lowerleg', 'rf_lowerleg', 'lb_lowerleg', 'rb_lowerleg'] if link in self.link_names] @@ -61,12 +61,12 @@ if __name__ == "__main__": robot = OpenDog(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider(robot.getLeftFrontLegIds()) + # robot.add_joint_slider(robot.getLeftFrontLegIds()) # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/pepper.py b/pyrobolearn/robots/pepper.py index 035d93d..c5c45c8 100644 --- a/pyrobolearn/robots/pepper.py +++ b/pyrobolearn/robots/pepper.py @@ -23,22 +23,22 @@ class Pepper(WheeledRobot, BiManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.9), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.9), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/pepper/pepper.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/pepper/pepper.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.9) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.9,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.9) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.9,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Pepper, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase) + super(Pepper, self).__init__(simulator, urdf, position, orientation, fixed_base) self.name = 'pepper' # 2D Camera sensor @@ -46,19 +46,19 @@ class Pepper(WheeledRobot, BiManipulatorRobot): # 2560x1080 at 5 frames per second. VFOV = 44.30 deg, HFOV= 57.20 deg, focus = [30cm, infinity] # Note that we divide width and height by 4 (otherwise the simulator is pretty slow) - self.cameraTop = CameraSensor(self.sim, self.id, 4, width=2560/4, height=1080/4, fovy=44.30, - near=0.3, far=100, refresh_rate=60) - self.cameraBottom = CameraSensor(self.sim, self.id, 9 ,width=2560/4, height=1080/4, fovy=44.30, - near=0.3, far=100, refresh_rate=60) + self.camera_top = CameraSensor(self.sim, self.id, 4, width=2560 / 4, height=1080 / 4, fovy=44.30, + near=0.3, far=100, refresh_rate=60) + self.camera_bottom = CameraSensor(self.sim, self.id, 9, width=2560 / 4, height=1080 / 4, fovy=44.30, + near=0.3, far=100, refresh_rate=60) # 3D camera sensor # From [1]: "One 3D camera is located in the forehead. It provides image resolution up to 320x240 at # 20 frames per second. One ASUS Xtion 3D sensor is located behind the eyes. VFOV = 45 deg, HFOV = 58 deg, # focus = [80cm, 3.5m]." - self.cameraDepth = CameraSensor(self.sim, self.id, 6, width=320, height=240, fovy=45, near=0.3, far=3.5, - refresh_rate=120) + self.camera_depth = CameraSensor(self.sim, self.id, 6, width=320, height=240, fovy=45, near=0.3, far=3.5, + refresh_rate=120) - self.cameras = [self.cameraTop, self.cameraBottom, self.cameraDepth] + self.cameras = [self.camera_top, self.camera_bottom, self.camera_depth] # Test @@ -77,14 +77,14 @@ if __name__ == "__main__": robot = Pepper(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for i in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() if i % 20 == 0: - robot.cameraTop.getRGBImage() + robot.camera_top.get_rgb_image() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/phantomx.py b/pyrobolearn/robots/phantomx.py index 0cfb202..17974a6 100644 --- a/pyrobolearn/robots/phantomx.py +++ b/pyrobolearn/robots/phantomx.py @@ -16,22 +16,22 @@ class PhantomX(HexapodRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.2), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.2), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/phantomx/phantomx.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/phantomx/phantomx.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.2) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.2,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.2) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.2,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(PhantomX, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(PhantomX, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'phantomx' @@ -51,7 +51,7 @@ if __name__ == "__main__": robot = PhantomX(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # run simulation for i in count(): diff --git a/pyrobolearn/robots/pleurobot.py b/pyrobolearn/robots/pleurobot.py index fee66d0..351ae4e 100644 --- a/pyrobolearn/robots/pleurobot.py +++ b/pyrobolearn/robots/pleurobot.py @@ -17,19 +17,19 @@ class Pleurobot(QuadrupedRobot, UUVRobot, USVRobot): [2] https://biorob.epfl.ch/pleurobot """ - def __init__(self, simulator, init_pos=(0, 0, 0), init_orient=(0, 0, 0, 1), useFixedBase=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/pleurobot/pleurobot.urdf'): + def __init__(self, simulator, position=(0, 0, 0), orientation=(0, 0, 0, 1), fixed_base=False, scaling=1., + urdf=os.path.dirname(__file__) + '/urdfs/pleurobot/pleurobot.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Pleurobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Pleurobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'pleurobot' @@ -49,14 +49,14 @@ if __name__ == "__main__": robot = Pleurobot(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() - # robot.computeAndDrawCoMPosition() - # robot.computeAndDrawProjectedCoMPosition() + # robot.update_joint_slider() + # robot.compute_and_draw_com_position() + # robot.compute_and_draw_projected_com_position() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/pr2.py b/pyrobolearn/robots/pr2.py index 4bcfc4c..ef8ce47 100644 --- a/pyrobolearn/robots/pr2.py +++ b/pyrobolearn/robots/pr2.py @@ -15,22 +15,22 @@ class PR2(WheeledRobot, BiManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/pr2/pr2.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/pr2/pr2.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(PR2, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(PR2, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'pr2' @@ -50,12 +50,12 @@ if __name__ == "__main__": robot = PR2(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/quadcopter.py b/pyrobolearn/robots/quadcopter.py index 6718b01..309b95a 100644 --- a/pyrobolearn/robots/quadcopter.py +++ b/pyrobolearn/robots/quadcopter.py @@ -55,22 +55,22 @@ class Quadcopter(RotaryWingUAV): def __init__(self, simulator, - init_pos=(0, 0, 0.2), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.2), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/quadcopter/quadcopter.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/quadcopter/quadcopter.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.2) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.2,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.2) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.2,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Quadcopter, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Quadcopter, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'quadcopter' self.gravity = 9.81 @@ -134,30 +134,30 @@ class Quadcopter(RotaryWingUAV): diameter = (4. * area / np.pi)**0.5 return air_density * area * (tmp**2 - tmp*v0) * (self.k1 * diameter / propeller_pitch)**self.k2 - def setJointVelocities(self, velocity, jointId=None, maxVelocity=True, maxTorque=True): + def set_joint_velocities(self, velocities, joint_ids=None, maxVelocity=True, maxTorque=True): """ Set the joint velocities and apply the thrust force on the propeller link corresponding to the given joint id(s). Args: - velocity (float[4]): velocity of each propeller - jointId (int[4], None): Not used here + velocities (float[4]): velocity of each propeller + joint_ids (int[4], None): Not used here maxVelocity (bool): maxTorque (bool): Returns: None """ - if len(velocity) != 4: + if len(velocities) != 4: raise ValueError("Expecting a velocity for each propeller") - jointId = self.joints + joint_ids = self.joints # call parent method - super(Quadcopter, self).setJointVelocities(velocity, jointId, maxVelocity, maxTorque) + super(Quadcopter, self).set_joint_velocities(velocities, joint_ids, maxVelocity, maxTorque) # calculate thrust force of the given joints, and apply it on the link - for jnt, d, v in zip(jointId, self.turning_directions, velocity): + for jnt, d, v in zip(joint_ids, self.turning_directions, velocities): if maxVelocity and v > self.max_velocity: v = self.max_velocity @@ -174,7 +174,7 @@ class Quadcopter(RotaryWingUAV): # f = self.mass * self.gravity / 4. # apply force in the simulation - self.applyExternalForce([0,0,f], jnt, position=(0.,0.,0.)) + self.apply_external_force([0, 0, f], jnt, position=(0., 0., 0.)) def getStationaryJointVelocity(self): fg = self.mass * self.gravity / 4. @@ -207,7 +207,7 @@ if __name__ == "__main__": robot = Quadcopter(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() rpm = robot.getStationaryRPM() print("Stationary RPM: {}".format(rpm)) @@ -216,6 +216,6 @@ if __name__ == "__main__": # run simulation for i in count(): - robot.setJointVelocities(v) + robot.set_joint_velocities(v) # step in simulation world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/rhex.py b/pyrobolearn/robots/rhex.py index f5ac706..b74f7a8 100644 --- a/pyrobolearn/robots/rhex.py +++ b/pyrobolearn/robots/rhex.py @@ -23,28 +23,28 @@ class Rhex(HexapodRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.12), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.12), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/rhex/rhex.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/rhex/rhex.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.12) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.12,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.12) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.12,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Rhex, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Rhex, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'rhex' - self.legs = [[self.getLinkIds(link + str(idx))] for link, idx in zip(['leg']*6, range(1, 7)) + self.legs = [[self.get_link_ids(link + str(idx))] for link, idx in zip(['leg'] * 6, range(1, 7)) if link + str(idx) in self.link_names] - self.feet = [self.getLinkIds(link + str(idx)) for link, idx in zip(['leg']*6, range(1, 7)) + self.feet = [self.get_link_ids(link + str(idx)) for link, idx in zip(['leg'] * 6, range(1, 7)) if link + str(idx) in self.link_names] self.leg_axis = np.ones(len(self.feet)) @@ -53,7 +53,7 @@ class Rhex(HexapodRobot): if isinstance(speed, (int, float)): speed = speed * np.ones(len(self.feet)) speed = speed * self.leg_axis - self.setJointVelocities(speed, self.feet) + self.set_joint_velocities(speed, self.feet) # Test @@ -72,14 +72,14 @@ if __name__ == "__main__": robot = Rhex(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider(robot.right_back_leg) + # robot.add_joint_slider(robot.right_back_leg) # run simulation for i in count(): - # robot.updateJointSlider() + # robot.update_joint_slider() robot.drive(2) # step in simulation world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py index 6150e5b..e7ab118 100644 --- a/pyrobolearn/robots/robot.py +++ b/pyrobolearn/robots/robot.py @@ -12,11 +12,12 @@ Dependencies: # import rbdl import numpy as np -import quaternion +# import quaternion import collections import os -from pyrobolearn.utils.converter import NumpyListConverter, QuaternionListConverter +from pyrobolearn.utils.orientation import * +from pyrobolearn.robots.base import ControllableBody __author__ = "Brian Delhaisse" @@ -29,59 +30,66 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class Robot(object): +class Robot(ControllableBody): r"""Robot class. This is the class that all robots should inherit from. It contains all the useful methods to operate the robot, and has been implemented such that it is very generic. """ - def __init__(self, simulator, urdf_path, init_pos=(0, 0, 1.5), init_orient=(0, 0, 0, 1), - useFixedBase=False, scaling=1., *args, **kwargs): + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1., *args, **kwargs): """ Initialize the robot. Args: simulator: reference to the simulator such that the robot can access it. - urdf_path (str): path to the urdf/mjcf file - init_pos (float[3]): initial position - init_orient (float[4]): initial orientation represented as a quaternion (x,y,z,w) - useFixedBase (bool): if True, the base of the robot will be fixed + urdf (str): path to the URDF/MJCF file. + position (np.float[3]): initial position. + orientation (np.float[4]): initial orientation represented as a quaternion (x,y,z,w). + fixed_base (bool, None): if True, the base of the robot will be fixed. scaling (float): scaling factor. """ # check parameters - if init_pos is None: - init_pos = (0., 0., 1.5) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 1.5) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - self.init_position = init_pos - self.init_orientation = init_orient - - # set the simulator - self.sim = simulator - # self.name = urdf_path.split('/')[-1].split('.urdf')[0] + # call parent constructor + super(Robot, self).__init__(simulator) # load the robot - # self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_RENDERING, 0) - if urdf_path[-3:] == 'xml' or urdf_path[-4:] == 'mjcf': - self.id = self.sim.loadMJCF(urdf_path)[0] # assume the first entity is the robot - else: # if urdf_path[-4:] == 'urdf': - self.id = self.sim.loadURDF(urdf_path, init_pos, init_orient, useFixedBase=useFixedBase, - globalScaling=scaling) + # self.sim.configure_debug_visualizer(self.sim.COV_ENABLE_RENDERING, 0) - # self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_RENDERING, 1) + if urdf[-3:] == 'xml' or urdf[-4:] == 'mjcf': + self.id = self.sim.load_mjcf(urdf)[0] # assume the first entity is the robot + else: # if urdf[-4:] == 'urdf': + self.id = self.sim.load_urdf(urdf, position, orientation, use_fixed_base=fixed_base, scale=scaling) + # self.sim.configure_debug_visualizer(self.sim.COV_ENABLE_RENDERING, 1) + + # rescale if specified if scaling != 1.0: # we rescale manually the mass and inertia matrices of each link - for link in range(self.getNumberOfLinks()): - info = self.sim.getDynamicsInfo(self.id, link) - mass, localInertiaDiagonal = info[0], np.array(info[2]) + for link in range(self.num_links): + info = self.sim.get_dynamics_info(self.id, link) + mass, local_inertia_diagonal = info[0], np.array(info[2]) mass *= scaling**3 # because the density is unchanged when scaling - localInertiaDiagonal *= scaling**5 # 5 = 3+2; 3 is for the mass, and 2 is for the distance: I~mr^2 - self.sim.changeDynamics(self.id, link, mass=mass, localInertiaDiagonal=localInertiaDiagonal) + local_inertia_diagonal *= scaling**5 # 5 = 3+2; 3 is for the mass, and 2 is for the distance: I~mr^2 + self.sim.change_dynamics(self.id, link, mass=mass, local_inertia_diagonal=local_inertia_diagonal) + + # set robot properties + self.init_position = position + self.init_orientation = orientation + self.base_height = self.get_base_position()[2] + self.base_up_vector = self.up_vector + self.base_forward_vector = self.forward_vector + # print("BASE HEIGHT: {}".format(self.base_height)) + # print("UP VECTOR: {}".format(self.base_up_vector)) + # print("FORWARD VECTOR: {}".format(self.base_forward_vector)) + self.com = None # center of mass # set useful variables self.joints = [] # non-fixed joint/link indices in the simulator @@ -89,49 +97,43 @@ class Robot(object): self.link_names = {} # link name to id in the simulator self.end_effectors = [] # end effector indices self.end_effector_names = {} # end effector name to id in the simulator - self.com = None # center of mass self.actuators = [] # list of actuators self.sensors = [] # list of sensors - for joint in range(self.getNumberOfJoints()): + # get actuated joints + for joint_id in range(self.num_joints): # Get joint info - jnt = self.sim.getJointInfo(self.id, joint) - self.joint_names[jnt[1]] = jnt[0] - self.link_names[jnt[12]] = jnt[0] + joint_info = self.sim.get_joint_info(self.id, joint_id) + self.joint_names[joint_info[1]] = joint_info[0] + self.link_names[joint_info[12]] = joint_info[0] - if jnt[2] != self.sim.JOINT_FIXED: # if not a fixed joint - self.joints.append(jnt[0]) + if joint_info[2] != self.sim.JOINT_FIXED: # if not a fixed joint + self.joints.append(joint_info[0]) # set automatically the end-effectors - self._setEndEffectors() + self._set_end_effectors() # visual debug: sliders and drawing self.joint_sliders = {} - self.comVisual = None - self.projectedCoMVisual = None - - # Converters - self.linear_converter = NumpyListConverter() - self.angular_converter = QuaternionListConverter(convention=1) + self.com_visual = None + self.projected_com_visual = None # other variables - self.coriolisAndGravityCompensation = False - self.floating_base = self._checkFloatingBase() + self.coriolis_and_gravity_compensation = False + self.floating_base = self._check_floating_base() # remember visual shapes # warning: the length of the returned list might be different from the number of links, because some links # don't have any visual shapes - visualShapes = self.sim.getVisualShapeData(self.id) - self.visualShapes = {shape[1]: {'dimensions': shape[3], 'color': list(shape[-1])} for shape in visualShapes} + visual_shapes = self.sim.get_visual_shape_data(self.id) + self.visual_shapes = {shape[1]: {'dimensions': shape[3], 'color': list(shape[7])} for shape in visual_shapes} # symbolic equations self.symbols = None - self._mass = None - # init joint positions - self.init_joint_positions = self.getJointPositions() - self.joint_limits = self.getJointLimits() + self.init_joint_positions = self.get_joint_positions() + self.joint_limits = self.get_joint_limits() # Gains self.kp, self.kd = None, None @@ -145,163 +147,101 @@ class Robot(object): """ return self.__class__.__name__ - def _convert_to_quat(self, quat, format='xyzw'): - """ - Convert quaternion from tuple/list/array to np.quaternion (w,x,y,z). - In pybullet, the quaternions are in the format (x,y,z,w). - :param quat_xyzw: tuple/list/array representing the quaternion - :param format: describe the format of the given quaternion - :return: np.quaternion - """ - if format == 'xyzw': - return np.quaternion(quat[3], *quat[:3]) - elif format == 'wxyz': - return np.quaternion(*quat) - else: - raise NotImplementedError('Unknown format for the given quaternion.') - ############## # Properties # ############## @property - def mass(self): - if not self._mass: # compute and cache the mass - self._mass = self.getTotalMass() - return self._mass - - ################ - # General Info # - ################ - - def getNumberOfDoFs(self): - """ - Return the number of degrees of freedom (i.e. the number of joints that are not fixed) - - Returns: - int: the number of degrees of freedom - """ + def num_dofs(self): + """Return the number of degrees of freedom (i.e. the number of joints that are not fixed)""" return len(self.joints) - def getTotalMass(self): - """ - Return the total mass of the robot (=sum of all mass links). - - Returns: - float: total mass of the robot [kg] - """ - return np.sum(self.getLinkMasses([-1] + list(range(self.getNumberOfLinks())))) - ######## # Base # ######## - def getBaseId(self): - """ - Return the base id. - - Returns: - int: base id. - """ - return -1 - - def getBaseName(self): - """ - Return the base name. - - Returns: - str: base name - """ - return self.sim.getBodyInfo(self.id)[0] - - def getBasePositionAndOrientation(self, convert_to_numpy_quaternion=True): + def get_base_pose(self): """ Get base position and orientation with respect to the world frame. Returns: float[3]: position - np.quaternion: orientation + np.float[4]: orientation """ - pos, orientation = self.sim.getBasePositionAndOrientation(self.id) - if convert_to_numpy_quaternion: - orientation = self._convert_to_quat(orientation) - else: - orientation = np.array(orientation) - return np.array(pos), orientation + return self.sim.get_base_pose(self.id) - def getBasePosition(self): + def get_base_position(self): """ Return the base position. Returns: float[3]: base position. """ - return np.array(self.sim.getBasePositionAndOrientation(self.id)[0]) + return self.sim.get_base_position(self.id) - def getBaseOrientation(self, convert_to_numpy_quaternion=True): + def get_base_orientation(self): """ Get the base orientation. Returns: - quaternion (float[4]): base orientation in the form of a quaternion. + quaternion (np.float[4]): base orientation in the form of a quaternion. """ - if convert_to_numpy_quaternion: - return self._convert_to_quat(self.sim.getBasePositionAndOrientation(self.id)[1]) - return np.array(self.sim.getBasePositionAndOrientation(self.id)[1]) + return self.sim.get_base_orientation(self.id) - def getBaseVelocity(self, concatenate=True): + def get_base_velocity(self, concatenate=True): """ Return the base linear and angular velocities. Returns: - float[6]: linear and angular velocities of the base + np.float[6]: linear and angular velocities of the base """ - lin_vel, ang_vel = self.sim.getBaseVelocity(self.id) + lin_vel, ang_vel = self.sim.get_base_velocity(self.id) if concatenate: - return np.array(lin_vel + ang_vel) - return np.array(lin_vel), np.array(ang_vel) + return np.concatenate((lin_vel, ang_vel)) + return lin_vel, ang_vel - def getBaseLinearVelocity(self): + def get_base_linear_velocity(self): """ Return the linear velocity of the base. Returns: float[3]: linear velocity of the base """ - return np.array(self.sim.getBaseVelocity(self.id)[0]) + return self.sim.get_base_linear_velocity(self.id) - def getBaseAngularVelocity(self): + def get_base_angular_velocity(self): """ Return the angular velocity of the base. Returns: float[3]: angular velocity of the base """ - return np.array(self.sim.getBaseVelocity(self.id)[1]) + return self.sim.get_base_angular_velocity(self.id) - def _checkFloatingBase(self): + def _check_floating_base(self): """ Return True if the robot has a floating base (i.e. floating root link). Otherwise, it is a fixed base. Returns: bool: True if the robot has a floating base. """ - # # We used the fact if the robot has a floating base then the base velocity can be close to 0, but never - # # completely equal to 0, unless the base is fixed - # return np.all(np.zeros(6) == self.getBaseVelocity()) + # We used the fact if the robot has a floating base then the base velocity can be close to 0, but never + # completely equal to 0, unless the base is fixed + # return np.all(np.zeros(6) == self.get_base_velocity()) # We check by computing the Jacobian (hopefully this only needs to be done once) if not self.joints: return False - linkId = self.joints[0] - J = self.calculateJacobian(linkId) + link_id = self.joints[0] + jacobian = self.get_jacobian(link_id) + # if floating base then the Jacobian will also include columns corresponding to the root link DoFs, while # with a fixed base, it will only have columns associated with the joints. - if J.shape[1] > len(self.joints): + if jacobian.shape[1] > len(self.joints): return True return False - def hasFloatingBase(self): + def has_floating_base(self): """ Return True if the robot has a floating base (i.e. floating root link). Otherwise, it is a fixed base. @@ -310,123 +250,92 @@ class Robot(object): """ return self.floating_base - def hasFixedBase(self): + def has_fixed_base(self): """ Return True if the robot has a fixed base. Returns: bool: True if the robot has a fixed base. """ - return not self.hasFloatingBase() + return not self.has_floating_base() ####### # CoM # ####### - def getCoMPosition(self): + def get_center_of_mass_position(self): """ Return the center of mass position. Returns: - float[3]: center of mass position + np.float[3]: center of mass position """ - linkIds = list(range(self.getNumberOfLinks())) - pos = self.getLinkWorldPositions(linkId=linkIds, flatten=False) - mass = self.getLinkMasses(linkId=linkIds) - - self.com = np.sum(pos.T * mass, axis=1) / np.sum(mass) - + self.com = self.sim.get_center_of_mass_position(self.id) return self.com - def getCoMVelocity(self): + def get_center_of_mass_velocity(self): """ Return the center of mass velocity. Returns: float[3]: center of mass velocity """ - linkIds = list(range(self.getNumberOfLinks())) - vel = self.getLinkWorldVelocities(linkId=linkIds, flatten=False) - mass = self.getLinkMasses(linkId=linkIds) + return self.sim.get_center_of_mass_velocity(self.id) - com = np.sum(vel.T * mass, axis=1) / np.sum(mass) - - return com - - def getLinearMomentum(self): - """ - Compute the linear momentum around the center of mass. - - .. math:: p = mv - - where :math:`p` is the linear momentum, :math:`m` is the total mass, and :math:`v` is the velocity. - - Returns: - np.array[3]: linear momentum - """ - return self.mass * self.getBaseLinearVelocity() - - def getAngularMomentum(self): - """ - Compute the angular momentum around the center of mass. - - .. math:: h = I\omega - - where :math:`h` is the angular momentum (based on the world origin), :math:`I` is the moment of inertia, - and :math:`\omega` is the angular velocity. - - Returns: - np.array[3]: angular momentum - """ - pass - - def getCentroidalDynamics(self, q=None, dq=None): - """ - Compute the centroidal momentum dynamics based on [1]. "The centroidal momentum of a rigid-body system - consists of its net linear momentum as well as its net angular momentum about its center of mass (CoM)" [1] - - #TODO: add math - - Args: - q (float[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will - get the current joint positions (but note that this could lead to a decrease of performance). - dq (float[M], None): joint velocities of size M (with 0 < M <= N). If None, it will - get the current joint velocities (but note that this could lead to a decrease of performance). - - Returns: - np.array[6, N+6]: centroidal momentum matrix :math:`A_G` - np.array[6]: the dot product between the derivative of the centroidal momentum matrix with the - generalized velocities vector. That is, :math:`\dot{A}_G \dot{q}` - - References: - [1] "Improved computation of the humanoid centroidal dynamics and application for whole-body control", - Wensing and Orin, 2016 - """ - pass + # def get_linear_momentum(self): + # """ + # Compute the linear momentum around the center of mass. + # + # .. math:: p = mv + # + # where :math:`p` is the linear momentum, :math:`m` is the total mass, and :math:`v` is the velocity. + # + # Returns: + # np.array[3]: linear momentum + # """ + # return self.mass * self.get_base_linear_velocity() + # + # def get_angular_momentum(self): + # """ + # Compute the angular momentum around the center of mass. + # + # .. math:: h = I\omega + # + # where :math:`h` is the angular momentum (based on the world origin), :math:`I` is the moment of inertia, + # and :math:`\omega` is the angular velocity. + # + # Returns: + # np.array[3]: angular momentum + # """ + # pass + # + # def get_centroidal_dynamics(self, q=None, dq=None): + # """ + # Compute the centroidal momentum dynamics based on [1]. "The centroidal momentum of a rigid-body system + # consists of its net linear momentum as well as its net angular momentum about its center of mass (CoM)" [1] + # + # Args: + # q (float[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will + # get the current joint positions (but note that this could lead to a decrease of performance). + # dq (float[M], None): joint velocities of size M (with 0 < M <= N). If None, it will + # get the current joint velocities (but note that this could lead to a decrease of performance). + # + # Returns: + # np.array[6, N+6]: centroidal momentum matrix :math:`A_G` + # np.array[6]: the dot product between the derivative of the centroidal momentum matrix with the + # generalized velocities vector. That is, :math:`\dot{A}_G \dot{q}` + # + # References: + # [1] "Improved computation of the humanoid centroidal dynamics and application for whole-body control", + # Wensing and Orin, 2016 + # """ + # pass ######################## # Joints (joint space) # ######################## - def getNumberOfJoints(self): - """ - Return the total number of joints (including fixed-ones). - - Returns: - int: the total number of joints (all types included) - """ - return self.sim.getNumJoints(self.id) - - def getNumberOfActuatedJoints(self): - """ - Return the total number of actuated joints. - - Returns: - int: the total number of actuated joints - """ - return len(self.joints) - - def getJointIds(self, joint=None): + def get_joint_ids(self, joint=None): """ Return the joint id(s) from the name(s) or q index(ices). @@ -447,22 +356,22 @@ class Robot(object): if joint is None: return self.joints - def getIndex(jnt): - if isinstance(jnt, str): - return self.joint_names[jnt] - elif isinstance(jnt, int): - return self.joints[jnt] + def get_index(joint): + if isinstance(joint, str): + return self.joint_names[joint] + elif isinstance(joint, int): + return self.joints[joint] else: raise TypeError("Incorrect type") # list of joints if isinstance(joint, collections.Iterable) and not isinstance(joint, str): - return [getIndex(jnt) for jnt in joint] + return [get_index(joint) for joint in joint] # one joint - return getIndex(joint) + return get_index(joint) - def getJointInfo(self, jointId=None): + def get_joint_info(self, joint_ids=None): """ Get information about the given joint(s). @@ -470,7 +379,7 @@ class Robot(object): only the desired information. Also, note that we do not convert the data here. Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, return the information for all (actuated) joints. Returns: @@ -499,13 +408,13 @@ class Robot(object): if multiple joints: list of joint information (i.e. list of above) """ - if isinstance(jointId, int): - return self.sim.getJointInfo(self.id, jointId) - if jointId is None: - jointId = self.joints - return [self.sim.getJointInfo(self.id, jnt) for jnt in jointId] + if isinstance(joint_ids, int): + return self.sim.get_joint_info(self.id, joint_ids) + if joint_ids is None: + joint_ids = self.joints + return [self.sim.get_joint_info(self.id, joint_id) for joint_id in joint_ids] - def getJointAxis(self, jointId=None): + def get_joint_axes(self, joint_ids=None): """ Get information about the given joint(s). @@ -513,28 +422,25 @@ class Robot(object): only the desired information. Also, note that we do not convert the data here. Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the axis for all + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, return the axis for all (actuated) joints. Returns: if 1 joint: - float[3]: joint axis + np.float[3]: joint axis if multiple joint: - [float[3]]: list of joint axis + [np.float[3]]: list of joint axis """ - if isinstance(jointId, int): - return self.sim.getJointInfo(self.id, jointId)[-4] - if jointId is None: - jointId = self.joints - return [self.sim.getJointInfo(self.id, jnt)[-4] for jnt in jointId] + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_axes(self.id, joint_ids) - # TODO: to check - def getQIndex(self, jointId=None): + def get_q_indices(self, joint_ids=None): """ Get the corresponding q index of the given joint(s). Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the q indices for all + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, return the q indices for all (actuated) joints. Returns: @@ -543,204 +449,54 @@ class Robot(object): if multiple joints: int[N]: q indices """ - if isinstance(jointId, int): - return self.sim.getJointInfo(self.id, jointId)[3] - 7 # TODO: check for 7 - if jointId is None: - jointId = self.joints - return np.array([self.sim.getJointInfo(self.id, jnt)[3] for jnt in jointId]) - 7 # TODO: check for 7 + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_q_indices(self.id, joint_ids) - def getDQIndex(self, jointId=None): - """ - Get the corresponding dq index of the given joint(s). - - Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the dq indices for all - (actuated) joints. - - Returns: - if 1 joint: - int: dq index - if multiple joints: - int[N]: dq indices - """ - if isinstance(jointId, int): - return self.sim.getJointInfo(self.id, jointId)[4] - if jointId is None: - jointId = self.joints - return [self.sim.getJointInfo(self.id, jnt)[4] for jnt in jointId] - - def getJointTypes(self, jointId=None, convert_to_string=True): + def get_joint_types(self, joint_ids=None, to_string=True): """ Get the joint type as a string or integer. Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, return the information for all (actuated) joints. - convert_to_string (bool): if True, it will return the joint type in a readable string format + to_string (bool): if True, it will return the joint type in a readable string format Returns: if 1 joint: str/int: the name of the joint type, or the flag associated with it. if multiple joints: list of above """ - if isinstance(jointId, int): - if convert_to_string: - return self.getJointTypeStr(self.sim.getJointInfo(self.id, jointId)[2]) - return self.sim.getJointInfo(self.id, jointId)[2] - if jointId is None: - jointId = self.joints - if convert_to_string: - return [self.getJointTypeStr(self.sim.getJointInfo(self.id, jnt)[2]) for jnt in jointId] - return [self.sim.getJointInfo(self.id, jnt)[2] for jnt in jointId] + if joint_ids is None: + joint_ids = self.joints + if to_string: + return self.sim.get_joint_type_names(self.id, joint_ids) + return self.sim.get_joint_type_ids(self.id, joint_ids) - def getJointTypeStr(self, idx): - """ - Return the joint type as a string based on the flag. - - Args: - idx (int): flag for the type of joint - - Returns: - str: name of the joint type - """ - return ['revolute', 'prismatic', 'spherical', 'planar', 'fixed', 'point2point', 'gear'][idx] - - def _isJointType(self, jointId=None, jointType=0): - """ - Return True if the given joint(s) are revolute. - - Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all - (actuated) joints. - jointType (int): flag for the type of joint. 0 = revolute, 1 = prismatic, 2 = spherical, 3 = planar, - 4 = fixed, 5 = point2point, 6 = gear. - - Returns: - bool, bool[N]: list of booleans. True if the joint(s) are revolute. - """ - if isinstance(jointId, int): - return (self.sim.getJointInfo(self.id, jointId)[2] == jointType) - if jointId is None: - jointId = self.joints - return [(self.sim.getJointInfo(self.id, joint)[2] == jointType) for joint in jointId] - - def isRevoluteJoint(self, jointId=None): - """ - Return True if the given joint(s) are revolute. - - Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all - (actuated) joints. - - Returns: - bool, bool[N]: list of booleans. True if the joint(s) are revolute. - """ - return self._isJointType(jointId, jointType=self.sim.JOINT_REVOLUTE) - - def isPrismaticJoint(self, jointId=None): - """ - Return True if the given joint(s) are prismatic. - - Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all - (actuated) joints. - - Returns: - bool, bool[N]: list of booleans. True if the joint(s) are prismatic. - """ - return self._isJointType(jointId, jointType=self.sim.JOINT_PRISMATIC) - - def isSphericalJoint(self, jointId=None): - """ - Return True if the given joint(s) are spherical. - - Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all - (actuated) joints. - - Returns: - bool, bool[N]: list of booleans. True if the joint(s) are spherical. - """ - return self._isJointType(jointId, jointType=self.sim.JOINT_SPHERICAL) - - def isPlanarJoint(self, jointId=None): - """ - Return True if the given joint(s) are planar. - - Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all - (actuated) joints. - - Returns: - bool, bool[N]: list of booleans. True if the joint(s) are planar. - """ - return self._isJointType(jointId, jointType=self.sim.JOINT_PLANAR) - - def isFixedJoint(self, jointId=None): - """ - Return True if the given joint(s) are fixed. - - Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all - (actuated) joints. - - Returns: - bool, bool[N]: list of booleans. True if the joint(s) are fixed. - """ - return self._isJointType(jointId, jointType=self.sim.JOINT_FIXED) - - def isPoint2PointJoint(self, jointId=None): - """ - Return True if the given joint(s) are point 2 point. - - Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all - (actuated) joints. - - Returns: - bool, bool[N]: list of booleans. True if the joint(s) are point-2-point. - """ - return self._isJointType(jointId, jointType=self.sim.JOINT_POINT2POINT) - - def isGearJoint(self, jointId=None): - """ - Return True if the given joint(s) are gear. - - Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all - (actuated) joints. - - Returns: - bool, bool[N]: list of booleans. True if the joint(s) are gear. - """ - return self._isJointType(jointId, jointType=self.sim.JOINT_GEAR) - - def getJointLimits(self, jointId=None): + def get_joint_limits(self, joint_ids=None): """ Get the joint limits of the given joint(s). Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, return the information for all (actuated) joints. Returns: if 1 joint: - float[2]: lower and upper limit + np.float[2]: lower and upper limit if multiple joints: - float[N,2]: lower and upper limit for each specified joint + np.float[N,2]: lower and upper limit for each specified joint """ - if isinstance(jointId, int): - return np.array(self.sim.getJointInfo(self.id, jointId)[8:10]) - if jointId is None: - jointId = self.joints - return np.array([self.sim.getJointInfo(self.id, jnt)[8:10] for jnt in jointId]) + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_limits(self.id, joint_ids) - def getJointDampings(self, jointId=None): + def get_joint_dampings(self, joint_ids=None): """ Get the damping coefficient of the given joint(s). Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, return the information for all (actuated) joints. Returns: @@ -749,62 +505,56 @@ class Robot(object): if multiple joints: float[N]: damping coefficient for each specified joint """ - if isinstance(jointId, int): - return self.sim.getJointInfo(self.id, jointId)[6] - if jointId is None: - jointId = self.joints - return np.array([self.sim.getJointInfo(self.id, jnt)[6] for jnt in jointId]) + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_dampings(self.id, joint_ids) - def getJointFrictions(self, jointId=None): + def get_joint_frictions(self, joint_ids=None): """ Get the friction coefficient of the given joint(s). Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, return the information for all (actuated) joints. Returns: if 1 joint: float: friction coefficient of the given joint if multiple joints: - float[N]: friction coefficient for each specified joint + np.float[N]: friction coefficient for each specified joint """ - if isinstance(jointId, int): - return self.sim.getJointInfo(self.id, jointId)[7] - if jointId is None: - jointId = self.joints - return np.array([self.sim.getJointInfo(self.id, jnt)[7] for jnt in jointId]) + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_frictions(self.id, joint_ids) - def getJointMaxForces(self, jointId=None): + def get_joint_max_forces(self, joint_ids=None): """ Get the maximum force that can be applied on the given joint(s). Warning: Note that this is not automatically used in position, velocity, or torque control. Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, return the information for all (actuated) joints. Returns: if 1 joint: float: maximum force [N] if multiple joints: - float[N]: maximum force for each specified joint [N] + np.float[N]: maximum force for each specified joint [N] """ - if isinstance(jointId, int): - return self.sim.getJointInfo(self.id, jointId)[10] - if jointId is None: - jointId = self.joints - return np.array([self.sim.getJointInfo(self.id, jnt)[10] for jnt in jointId]) + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_max_forces(self.id, joint_ids) - def getJointMaxVelocities(self, jointId=None): + def get_joint_max_velocities(self, joint_ids=None): """ Get the maximum velocity that can be applied on the given joint(s). Warning: Note that this is not automatically used in position, velocity, or torque control. Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, return the information for all + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, return the information for all (actuated) joints. Returns: @@ -813,18 +563,16 @@ class Robot(object): if multiple joints: float[N]: maximum velocities for each specified joint [rad/s] """ - if isinstance(jointId, int): - return self.sim.getJointInfo(self.id, jointId)[11] - if jointId is None: - jointId = self.joints - return np.array([self.sim.getJointInfo(self.id, jnt)[11] for jnt in jointId]) + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_max_velocities(self.id, joint_ids) - def getJointNames(self, jointId=None): + def get_joint_names(self, joint_ids=None): """ Return the name of the given joint(s). Args: - jointId (int, int[N]): joint id, or list of joint ids. If None, get the name of all (actuated) joints. + joint_ids (int, int[N]): joint id, or list of joint ids. If None, get the name of all (actuated) joints. Returns: if 1 joint: @@ -832,40 +580,38 @@ class Robot(object): if multiple joints: str[N]: name of each joint """ - if isinstance(jointId, int): - return self.sim.getJointInfo(self.id, jointId)[1] - if jointId is None: - jointId = self.joints - return [self.sim.getJointInfo(self.id, joint)[1] for joint in jointId] + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_names(self.id, joint_ids) - def getJointStates(self, jointId=None): + def get_joint_states(self, joint_ids=None): """ Get the state of the given joint(s). Args: - jointId (int, int[N], None): id of the joint, or list of joint ids. If None, get the state of all + joint_ids (int, int[N], None): id of the joint, or list of joint ids. If None, get the state of all (actuated) joints. Returns: for 1 joint: float: joint position [rad] float: joint velocity [rad/s] - float[6]: joint reaction forces [fx,fy,fz,mx,my,mz] + np.float[6]: joint reaction forces [fx,fy,fz,mx,my,mz] float: applied joint motor torque (during the last step) for multiple joints: list of each joint state """ - if isinstance(jointId, int): - return self.sim.getJointState(self.id, jointId) - if jointId is None: - jointId = self.joints - return self.sim.getJointStates(self.id, jointId) + if isinstance(joint_ids, int): + return self.sim.get_joint_state(self.id, joint_ids) + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_states(self.id, joint_ids) - def getJointPositions(self, jointId=None): + def get_joint_positions(self, joint_ids=None): """ Get the position of the given joint(s). Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, get the position of all (actuated) + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, get the position of all (actuated) joints. Returns: @@ -874,18 +620,16 @@ class Robot(object): if multiple joints: np.float[N]: joint positions [rad] """ - if isinstance(jointId, int): - return self.sim.getJointState(self.id, jointId)[0] - if jointId is None: - jointId = self.joints - return np.array([state[0] for state in self.sim.getJointStates(self.id, jointId)]) + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_positions(self.id, joint_ids) - def getJointVelocities(self, jointId=None): + def get_joint_velocities(self, joint_ids=None): """ Get the velocity of the given joint(s). Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, get the velocity of all (actuated) + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, get the velocity of all (actuated) joints. Returns: @@ -894,19 +638,17 @@ class Robot(object): if multiple joints: np.float[N]: joint velocities [rad/s] """ - if isinstance(jointId, int): - return self.sim.getJointState(self.id, jointId)[1] - if jointId is None: - jointId = self.joints - return np.array([state[1] for state in self.sim.getJointStates(self.id, jointId)]) + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_velocities(self.id, joint_ids) - def getJointAccelerations(self, jointId=None): + def get_joint_accelerations(self, joint_ids=None): """ Get the acceleration at the given joint(s). This is carried out by first getting the joint torques, then performing forward dynamics to get the joint accelerations from the joint torques. Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, get the acceleration of all + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, get the acceleration of all (actuated) joints. Returns: @@ -916,27 +658,27 @@ class Robot(object): np.float[N]: joint accelerations [rad/s^2] """ # check joint id - if jointId is None: - jointId = self.joints + if joint_ids is None: + joint_ids = self.joints # get the torques - torques = self.getJointTorques(jointId) + torques = self.get_joint_torques(joint_ids) # compute the accelerations - accelerations = self.calculateForwardDynamics(torques) + accelerations = self.calculate_forward_dynamics(torques) # return the specified accelerations - q_idx = self.getQIndex(jointId) + q_idx = self.get_q_indices(joint_ids) return accelerations[q_idx] - def getJointReactionForces(self, jointId=None): + def get_joint_reaction_forces(self, joint_ids=None): """ Return the joint reaction forces at the given joint. Note that the torque sensor must be enabled, otherwise it will always return [0,0,0,0,0,0]. Args: - jointId (int, int[N], None): unique id of the joint, or list of joint ids. If None, get the joint reaction - forces of all (actuated) joints. + joint_ids (int, int[N], None): unique id of the joint, or list of joint ids. If None, get the joint + reaction forces of all (actuated) joints. Returns: if 1 joint: @@ -944,21 +686,16 @@ class Robot(object): if multiple joints: np.float[N,6]: joint reaction forces [N, Nm] """ - if isinstance(jointId, int): - return np.array(self.sim.getJointState(self.id, jointId)[2]) - if jointId is None: - jointId = self.joints - return np.array([state[2] for state in self.sim.getJointStates(self.id, jointId)]) + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_reaction_forces(self.id, joint_ids) - def getJointTorques(self, jointId=None): + def get_joint_torques(self, joint_ids=None): """ - Get the applied torque on the given joint(s). "This is the motor torque applied during the last stepSimulation. - Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the - applied joint motor torque is exactly what you provide, so there is no need to report it separately." (from - the 'pybullet user guide') + Get the applied torque on the given joint(s). Args: - jointId (int, int[N], None): id of the joint, or list of joint ids. If None, get the joint torques of + joint_ids (int, int[N], None): id of the joint, or list of joint ids. If None, get the joint torques of all (actuated) joints. Returns: @@ -967,20 +704,17 @@ class Robot(object): if multiple joints: np.float[N]: torques associated to the given joints [Nm] """ - if isinstance(jointId, int): - return self.sim.getJointState(self.id, jointId)[3] - if jointId is None: - jointId = self.joints - return np.array([state[3] for state in self.sim.getJointStates(self.id, jointId)]) + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_torques(self.id, joint_ids) - def getJointPowers(self, jointId, torque=None): + def get_joint_powers(self, joint_ids=None): """ Return the applied power at the given joint(s). Power = torque * velocity. Args: - jointId (int, int[N]): joint id, or list of joint ids - torque (float, float[N]): torques to apply to the joint(s). This has to be provided if we are doing - TORQUE_CONTROL. + joint_ids (int, int[N], None): id of the joint, or list of joint ids. If None, get the joint powers of + all (actuated) joints. Returns: if 1 joint: @@ -988,162 +722,163 @@ class Robot(object): if multiple joints: np.float[N]: power at each joint [W] """ - if isinstance(torque, (list, tuple)): - torque = np.array(torque) - elif torque is None: - torque = self.getJointTorques(jointId) - velocity = self.getJointVelocities(jointId) - return torque * velocity + if joint_ids is None: + joint_ids = self.joints + return self.sim.get_joint_powers(self.id, joint_ids) - # TODO: desVel, maxVel, and maxTorque - def setJointPositions(self, position, jointId=None, kp=None, kd=None, velocity=None, maxTorque=None): + # TODO: max_velocities and forces + def set_joint_positions(self, positions, joint_ids=None, kp=None, kd=None, velocities=None, forces=None): """ Set the position of the given joint(s) (using position control). Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, get all the actuated joints. - position (float, float[N]): desired position, or list of desired positions [rad] - velocity (float, float[N], None): desired velocity, or list of desired velocities [rad/s] - kp (float, float[N], None): position gain(s) - kd (float, float[N], None): velocity gain(s) - maxTorque (float, float[N], None): maximum motor torques + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, get all the actuated joints. + positions (float, np.float[N]): desired position, or list of desired positions [rad] + velocities (float, np.float[N], None): desired velocity, or list of desired velocities [rad/s] + kp (float, np.float[N], None): position gain(s) + kd (float, np.float[N], None): velocity gain(s) + forces (float, np.float[N], None, bool): maximum motor torques / forces. If True, it will apply the + default maximum force values. """ - if isinstance(jointId, int): - kwargs = {} - if kp is not None: - kwargs['positionGain'] = kp - if kd is not None: - kwargs['velocityGain'] = kd - if velocity is not None: - kwargs['targetVelocity'] = velocity - if maxTorque is not None: - kwargs['force'] = maxTorque - self.sim.setJointMotorControl2(self.id, jointId, self.sim.POSITION_CONTROL, targetPosition=position, - **kwargs) - else: - if jointId is None: - jointId = self.joints - kwargs = {} - if kp is not None: - if isinstance(kp, (float, int)): - kp = kp * np.ones(len(jointId)) - kwargs['positionGains'] = kp - if kd is not None: - if isinstance(kd, (float, int)): - kd = kd * np.ones(len(jointId)) - kwargs['velocityGains'] = kd - # qIdx = self.getQIndex(jointId) - # print("pos: ", position) - # print(self.joint_limits[qIdx, 0], self.joint_limits[qIdx, 1]) - # TODO: the following clip causes an error... Check Minitaur... - # position = np.clip(position, self.joint_limits[qIdx, 0], self.joint_limits[qIdx, 1]) - # kp = kp.tolist() - # kd = kd.tolist() - # print("pos: ", position) - # print("kp: ", kp) - # print("kd: ", kd) - if velocity is not None: - if isinstance(velocity, (float, int)): - velocity = velocity * np.ones(len(jointId)) - kwargs['targetVelocities'] = velocity - if maxTorque is not None: - if isinstance(maxTorque, (float, int)): - maxTorque = maxTorque * np.ones(len(jointId)) - kwargs['forces'] = maxTorque - self.sim.setJointMotorControlArray(self.id, jointId, self.sim.POSITION_CONTROL, targetPositions=position, - **kwargs) + if joint_ids is None: + joint_ids = self.joints + self.sim.set_joint_positions(self.id, joint_ids, positions, velocities=velocities, kps=kp, kds=kd, + forces=forces) - # TODO: maxVel and maxTorque - def setJointVelocities(self, velocity, jointId=None, maxVelocity=True, maxTorque=True): + # if isinstance(joint_ids, int): + # kwargs = {} + # if kp is not None: + # kwargs['positionGain'] = kp + # if kd is not None: + # kwargs['velocityGain'] = kd + # if velocities is not None: + # kwargs['targetVelocity'] = velocities + # if forces is not None: + # kwargs['force'] = forces + # self.sim.setJointMotorControl2(self.id, joint_ids, self.sim.POSITION_CONTROL, position=positions, + # **kwargs) + # else: + # if joint_ids is None: + # joint_ids = self.joints + # kwargs = {} + # if kp is not None: + # if isinstance(kp, (float, int)): + # kp = kp * np.ones(len(joint_ids)) + # kwargs['positionGains'] = kp + # if kd is not None: + # if isinstance(kd, (float, int)): + # kd = kd * np.ones(len(joint_ids)) + # kwargs['velocityGains'] = kd + # # qIdx = self.get_q_indices(jointId) + # # print("pos: ", position) + # # print(self.joint_limits[qIdx, 0], self.joint_limits[qIdx, 1]) + # # TODO: the following clip causes an error... Check Minitaur... + # # position = np.clip(position, self.joint_limits[qIdx, 0], self.joint_limits[qIdx, 1]) + # # kp = kp.tolist() + # # kd = kd.tolist() + # # print("pos: ", position) + # # print("kp: ", kp) + # # print("kd: ", kd) + # if velocities is not None: + # if isinstance(velocities, (float, int)): + # velocities = velocities * np.ones(len(joint_ids)) + # kwargs['targetVelocities'] = velocities + # if forces is not None: + # if isinstance(forces, (float, int)): + # forces = forces * np.ones(len(joint_ids)) + # kwargs['forces'] = forces + # self.sim.setJointMotorControlArray(self.id, joint_ids, self.sim.POSITION_CONTROL, positions=positions, + # **kwargs) + + # TODO: max_velocities and forces + def set_joint_velocities(self, velocities, joint_ids=None, forces=None, max_velocity=None): """ Set the velocity of the given joint(s) (using velocity control). Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, get all the actuated joints. - velocity (float, float[N]): desired velocity, or list of desired velocities [rad/s] - maxVelocity (bool): if True, it will make sure that the given velocity(ies) are below their authorized - maximum value(s) (inferred from the URDF, or set previously by the user). If you already did the check - outside the method or if you don't want limits, set this variable to False. - maxTorque (bool, float, float[N]): maximum motor torques + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, get all the actuated joints. + velocities (float, float[N]): desired velocity, or list of desired velocities [rad/s] + forces (float, np.float[N], None, bool): maximum motor torques / forces. If True, it will apply the + default maximum force values. + max_velocity (float, bool, None): if True, it will make sure that the given velocity(ies) are below their + authorized maximum value(s) (inferred from the URDF, or set previously by the user). If you already + did the check outside the method or if you don't want limits, set this variable to False. """ - if isinstance(jointId, int): - self.sim.setJointMotorControl2(self.id, jointId, self.sim.VELOCITY_CONTROL, targetVelocity=velocity) - else: - if jointId is None: - jointId = self.joints - self.sim.setJointMotorControlArray(self.id, jointId, self.sim.VELOCITY_CONTROL, targetVelocities=velocity) + if joint_ids is None: + joint_ids = self.joints + self.sim.set_joint_motor_control(self.id, joint_ids, self.sim.VELOCITY_CONTROL, velocities=velocities) - # TODO: maxAccel and maxTorque - def setJointAccelerations(self, acceleration, jointId=None, maxAcceleration=True): + # TODO: max_acceleration + def set_joint_accelerations(self, accelerations, joint_ids=None, max_acceleration=True): """ Set the acceleration of the given joint(s) (using force control). This is achieved by performing inverse dynamic which given the joint accelerations compute the joint torques to be applied. Args: - acceleration (float, float[N]): desired joint acceleration, or list of desired joint accelerations [rad/s^2] - jointId (int, int[N], None): joint id, or list of joint ids. If None, get all the actuated joints. - maxAcceleration (bool): if True, it will make sure that the given acceleration(s) are below their - authorized maximum value(s). If you already did the check outside the method or if you don't want + accelerations (float, float[N]): desired joint acceleration, or list of desired joint accelerations [rad/s^2] + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, get all the actuated joints. + max_acceleration (bool, float, None): if True, it will make sure that the given acceleration(s) are below + their authorized maximum value(s). If you already did the check outside the method or if you don't want limits, set this variable to False. """ # check joint ids - if jointId is None: - jointId = self.joints - elif isinstance(jointId, int): - jointId = [jointId] - if isinstance(acceleration, (int, float)): - acceleration = [acceleration] - if len(acceleration) != len(jointId): + if joint_ids is None: + joint_ids = self.joints + elif isinstance(joint_ids, int): + joint_ids = [joint_ids] + if isinstance(accelerations, (int, float)): + accelerations = [accelerations] + if len(accelerations) != len(joint_ids): raise ValueError("Expecting the desired accelerations to be of the same size as the number of joints; " - "{} != {}".format(len(acceleration), len(jointId))) + "{} != {}".format(len(accelerations), len(joint_ids))) # if joint accelerations vector is not the same size as the actuated joints - if len(acceleration) != len(self.joints): - q_idx = self.getQIndex(jointId) + if len(accelerations) != len(self.joints): + q_idx = self.get_q_indices(joint_ids) acc = np.zeros(len(self.joints)) - acc[q_idx] = acceleration - acceleration = acc + acc[q_idx] = accelerations + accelerations = acc # compute joint torques from Inverse Dynamics - torques = self.calculateInverseDynamics(acceleration) + torques = self.calculate_inverse_dynamics(accelerations) # get corresponding torques - if len(torques) != len(jointId): - q_idx = self.getQIndex(jointId) + if len(torques) != len(joint_ids): + q_idx = self.get_q_indices(joint_ids) torques = torques[q_idx] # print("Robot - torques {} for joints {}".format(torques, jointId)) # set the joint torques - self.setJointTorques(torques, jointId) + self.set_joint_torques(torques, joint_ids) - def setJointTorques(self, torque=None, jointId=None): + def set_joint_torques(self, torque=None, joint_ids=None): """ Set the torque to the given joint(s) (using force/torque control). Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, it will set the joint torques to + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will set the joint torques to all (actuated) joints. torque (float, float[N], None): desired torque(s) to apply to the joint(s) [N]. If None, it will apply a torque of 0 to the given joint(s). """ - if isinstance(jointId, int): + if isinstance(joint_ids, int): if torque is None: torque = 0 - self.sim.setJointMotorControl2(self.id, jointId, self.sim.TORQUE_CONTROL, force=torque) else: - if jointId is None: - jointId = self.joints - if not isinstance(jointId, collections.Iterable): + if joint_ids is None: + joint_ids = self.joints + if not isinstance(joint_ids, collections.Iterable): raise TypeError("Expecting jointId to be a tuple, list, or numpy array, got instead " - "{}".format(type(jointId))) + "{}".format(type(joint_ids))) if torque is None: - torque = [0]*len(jointId) + torque = [0]*len(joint_ids) elif isinstance(torque, (int, float)): - torque = [torque]*len(jointId) - self.sim.setJointMotorControlArray(self.id, jointId, self.sim.TORQUE_CONTROL, forces=torque) + torque = [torque]*len(joint_ids) - def setJointMotorControl(self, jointId, **kwargs): + self.sim.set_joint_motor_control(self.id, joint_ids, self.sim.TORQUE_CONTROL, forces=torque) + + def set_joint_motor_control(self, joint_ids, control_mode, **kwargs): """ Set joint motor control. @@ -1156,89 +891,83 @@ class Robot(object): Note that the maximum forces and velocities are not automatically used for the different control schemes. Args: - jointId (int, int[N]): joint id, or list of joint ids + joint_ids (int, int[N]): joint id, or list of joint ids + control_mode (int): sim.VELOCITY_CONTROL (=0), sim.TORQUE_CONTROL (=1), sim.POSITION_CONTROL (=2) kwargs: - controlMode (int): sim.VELOCITY_CONTROL (=0), sim.TORQUE_CONTROL (=1), sim.POSITION_CONTROL (=2) - targetPosition (float, float[N]) (optional): target position of the joint (in position control) [rad] - targetVelocity (float, float[N]) (optional): target velocity of the joint (in position/velocity + positions (float, float[N]) (optional): target position of the joint (in position control) [rad] + velocities (float, float[N]) (optional): target velocity of the joint (in position/velocity control) [rad/s] - force (float, float[N]) (optional): in position/velocity control, this is the maximum force used + forces (float, float[N]) (optional): in position/velocity control, this is the maximum force used to reach the target value. In torque control, this is the force/torque to be applied. - positionGain (float, float[N]) (optional): position gain :math:`Kp` - velocityGain (float, float[N]) (optional): velocity gain :math:`Kd` + kp (float, float[N]) (optional): position gain :math:`Kp` + kd (float, float[N]) (optional): velocity gain :math:`Kd` maxVelocity (float, float[N]) (optional): in position control, this limits the velocity to a maximum. """ - if isinstance(jointId, int): - self.sim.setJointMotorControl2(self.id, jointId, **kwargs) - else: - self.sim.setJointMotorControlArray(self.id, jointId, **kwargs) + self.sim.set_joint_motor_control(self.id, joint_ids, control_mode, **kwargs) - def disableMotor(self, jointId=None): + def disable_motor(self, joint_ids=None): """ Disable the motor associated with the given joint(s). Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, it will disable the motors of + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will disable the motors of all actuated joints. """ - if isinstance(jointId, int): - self.sim.setJointMotorControl2(self.id, jointId, self.sim.VELOCITY_CONTROL, force=0) - else: - if jointId is None: - jointId = self.joints - self.sim.setJointMotorControlArray(self.id, jointId, self.sim.VELOCITY_CONTROL, forces=[0] * len(jointId)) + if joint_ids is None: + joint_ids = self.joints + self.sim.set_joint_motor_control(self.id, joint_ids, self.sim.VELOCITY_CONTROL, forces=[0] * len(joint_ids)) - def resetJointStates(self, q=None, dq=None, jointIds=None): + def reset_joint_states(self, q=None, dq=None, joint_ids=None): """ Reset the state of the robot. Warnings: This is only valid in the simulator, and note that calling this method overrides all physics simulation. """ - # check jointIds - if not jointIds: - jointIds = self.joints - if isinstance(jointIds, int): - jointIds = [jointIds] + # check joint_ids + if not joint_ids: + joint_ids = self.joints + if isinstance(joint_ids, int): + joint_ids = [joint_ids] # check q if q is None: - q = np.zeros(len(jointIds)) + q = np.zeros(len(joint_ids)) elif isinstance(q, (int, float)): q = [q] else: - if len(q) != len(jointIds): + if len(q) != len(joint_ids): raise ValueError("The number of joint ids does not match up with the number of q's") # check dq if dq is None: - dq = np.zeros(len(jointIds)) + dq = np.zeros(len(joint_ids)) elif isinstance(dq, (int, float)): dq = [dq] else: - if len(dq) != len(jointIds): + if len(dq) != len(joint_ids): raise ValueError("The number of joint ids does not match with the number of dq's") # reset the joint state - for jointId, p, v in zip(jointIds, q, dq): - self.sim.resetJointState(self.id, jointId, p, v) + for joint_id, position, velocity in zip(joint_ids, q, dq): + self.sim.reset_joint_state(self.id, joint_id, position, velocity) - def getHomeJointPositions(self): + def get_home_joint_positions(self): """ Return the joint positions for the home position defined by the user. This method has to be overwritten in the child class. """ - return np.zeros(self.getNumberOfActuatedJoints()) + return np.zeros(self.num_actuated_joints) - def setJointHomePositions(self): + def set_joint_home_positions(self): """ Set the joints to their home position defined by the user. """ - jointPositions = self.getHomeJointPositions() - if jointPositions is not None: - self.resetJointStates(jointPositions) + joint_positions = self.get_home_joint_positions() + if joint_positions is not None: + self.reset_joint_states(joint_positions) - def moveJointHomePositions(self): + def move_joint_home_positions(self): """ Move the joints to their home position defined by the user. This method can be overwritten in the child class. @@ -1246,36 +975,18 @@ class Robot(object): The difference between this method and the `setJointHomePosition` is that the latter directly (re)set the joints to their home position while this one moves the joints to their home position. """ - jointPositions = self.getHomeJointPositions() - if jointPositions is not None: - self.setJointPositions(jointPositions) + joint_positions = self.get_home_joint_positions() + if joint_positions is not None: + self.set_joint_positions(joint_positions) - def setJointInitPositions(self): - self.setJointPositions(self.init_joint_positions) + def set_joint_init_positions(self): + self.set_joint_positions(self.init_joint_positions) ################################## # Links (task/operational space) # ################################## - def getNumberOfLinks(self): - """ - Return the total number of links even the ones associated to fixed-joints. - - Returns: - int: the number of links - """ - return self.getNumberOfJoints() - - def getNumberOfActuatedLinks(self): - """ - Return the number of links associated to actuated joints. - - Returns: - int: the number of links - """ - return self.getNumberOfActuatedJoints() - - def getLinkIds(self, link=None): + def get_link_ids(self, link=None): """ Return the link id(s) from the name(s) or q index(ices). @@ -1297,27 +1008,27 @@ class Robot(object): if link is None: return self.joints - def getIndex(lnk): - if isinstance(lnk, str): - return self.link_names[lnk] - elif isinstance(lnk, int): - return self.joints[lnk] + def get_index(link): + if isinstance(link, str): + return self.link_names[link] + elif isinstance(link, int): + return self.joints[link] else: raise TypeError("Incorrect type") # list of links if isinstance(link, collections.Iterable) and not isinstance(link, str): - return [getIndex(lnk) for lnk in link] + return [get_index(link) for link in link] # one link - return getIndex(link) + return get_index(link) - def getParentLinkIds(self, linkId=None): + def get_parent_link_ids(self, link_ids=None): """ Return the parent link of the given link(s) Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the state of all links + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the state of all links associated to actuated joints. Returns: @@ -1326,84 +1037,84 @@ class Robot(object): if multiple links: int[N]: link ids """ - if isinstance(linkId, int): - return self.sim.getJointInfo(self.id, linkId)[-1] - if linkId is None: - linkId = self.joints - return [self.sim.getJointInfo(self.id, link)[-1] for link in linkId] + if isinstance(link_ids, int): + return self.sim.get_joint_info(self.id, link_ids)[-1] + if link_ids is None: + link_ids = self.joints + return [self.sim.get_joint_info(self.id, link)[-1] for link in link_ids] - def getChainLinkIds(self, toLinkId, fromLinkId=None): + def get_chain_link_ids(self, to_link_id, from_link_id=None): """ Return the link ids that constitute the chain(s) that go(es) from `fromLinkId` to `toLinkId`. Args: - toLinkId (int, int[M]): link id(s) that end(s) the chain(s). - fromLinkId (int, int[M], None): link id(s) that start(s) the chain(s). `fromLinkId` has to be a parent or + to_link_id (int, int[M]): link id(s) that end(s) the chain(s). + from_link_id (int, int[M], None): link id(s) that start(s) the chain(s). `fromLinkId` has to be a parent or ancestor of the `toLinkId`. If None, it will return the chain going from the base to the `toLinkId`. Returns: int[N], [int[N]]: chain(s) containing the link ids. """ - if fromLinkId is None: - if isinstance(toLinkId, collections.Iterable): - fromLinkId = [-1] * len(toLinkId) + if from_link_id is None: + if isinstance(to_link_id, collections.Iterable): + from_link_id = [-1] * len(to_link_id) else: - fromLinkId = -1 + from_link_id = -1 - def get_chain(toLink, fromLink): - chain = [toLink] - for linkId in chain: - linkId = self.getParentLinkIds(linkId) - chain.append(linkId) - if linkId == fromLinkId: + def get_chain(to_link, from_link): + chain = [to_link] + for link_id in chain: + link_id = self.get_parent_link_ids(link_id) + chain.append(link_id) + if link_id == from_link: break return chain[::-1] - if isinstance(toLinkId, int): - return get_chain(toLinkId, fromLinkId) + if isinstance(to_link_id, int): + return get_chain(to_link_id, from_link_id) else: - return [get_chain(toLink, fromLink) for toLink, fromLink in zip(toLinkId, fromLinkId)] + return [get_chain(to_link, from_link) for to_link, from_link in zip(to_link_id, from_link_id)] - def getLinkStates(self, linkId=None, computeLinkVelocity=True, computeForwardKinematics=True): + def get_link_states(self, link_ids=None, compute_link_velocity=True, compute_forward_kinematics=True): """ Return the state of the given link(s). Warning: note that we do not convert the data here. Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the state of all links + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the state of all links associated to actuated joints. - computeLinkVelocity (bool): if True, the Cartesian world velocity will be computed and returned. - computeForwardKinematics (bool): if True, the Cartesian world position/orientation will be recomputed + compute_link_velocity (bool): if True, the Cartesian world velocity will be computed and returned. + compute_forward_kinematics (bool): if True, the Cartesian world position/orientation will be recomputed using forward kinematics. Returns: if 1 link: - [0] float[3]: Cartesian position of center of mass - [1] float[4]: Cartesian orientation of center of mass - [2] float[3]: local position offset of inertial frame (CoM) expressed in the URDF link frame - [3] float[4]: local orientation (quat. [x,y,z,w]) offset of the inertial frame expressed in URDF link - frame - [4] float[3]: world position of the URDF link frame - [5] float[4]: world orientation of the URDF link frame - [6] float[3]: Cartesian world linear velocity - [7] float[3]: Cartesian world angular velocity + [0] np.float[3]: Cartesian position of center of mass + [1] np.float[4]: Cartesian orientation of center of mass + [2] np.float[3]: local position offset of inertial frame (CoM) expressed in the URDF link frame + [3] np.float[4]: local orientation (quat. [x,y,z,w]) offset of the inertial frame expressed in URDF + link frame + [4] np.float[3]: world position of the URDF link frame + [5] np.float[4]: world orientation of the URDF link frame + [6] np.float[3]: Cartesian world linear velocity + [7] np.float[3]: Cartesian world angular velocity if multiple links: list of above """ - if isinstance(linkId, int): # one link - return self.sim.getLinkState(self.id, linkId, computeLinkVelocity=computeLinkVelocity, - computeForwardKinematics=computeForwardKinematics) - if linkId is None: - linkId = self.joints - return [self.sim.getLinkState(self.id, link, computeLinkVelocity=computeLinkVelocity, - computeForwardKinematics=computeForwardKinematics) for link in linkId] + if isinstance(link_ids, int): # one link + return self.sim.get_link_state(self.id, link_ids, compute_velocity=compute_link_velocity, + compute_forward_kinematics=compute_forward_kinematics) + if link_ids is None: + link_ids = self.joints + return self.sim.get_link_states(self.id, link_ids, compute_velocity=compute_link_velocity, + compute_forward_kinematics=compute_forward_kinematics) - def getLinkNames(self, linkId=None): + def get_link_names(self, link_ids=None): """ Return the name of the given link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the name of all links + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the name of all links associated to actuated joints. Returns: @@ -1412,261 +1123,260 @@ class Robot(object): if multiple links: str[N]: link names """ - if isinstance(linkId, int): - return self.sim.getJointInfo(self.id, linkId)[12] - if linkId is None: - linkId = self.joints - return [self.sim.getJointInfo(self.id, link)[12] for link in linkId] + if isinstance(link_ids, int): + return self.sim.get_joint_info(self.id, link_ids)[12] + if link_ids is None: + link_ids = self.joints + return [self.sim.get_joint_info(self.id, link)[12] for link in link_ids] - def getLinkMasses(self, linkId=None): + def get_link_masses(self, link_ids=None): """ Return the mass of the given link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the mass of all the links + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the mass of all the links (even of fixed links). Returns: if 1 link: float: mass of the given link else: - float[N]: mass of each link + np.float[N]: mass of each link """ - if isinstance(linkId, int): - return self.sim.getDynamicsInfo(self.id, linkId)[0] - if linkId is None: - linkId = list(range(self.getNumberOfLinks())) - return np.array([self.sim.getDynamicsInfo(self.id, link)[0] for link in linkId]) + if isinstance(link_ids, int): + return self.sim.get_dynamics_info(self.id, link_ids)[0] + if link_ids is None: + link_ids = list(range(self.num_links)) + return np.array([self.sim.get_dynamics_info(self.id, link)[0] for link in link_ids]) - def getLinkFrames(self, linkId=None, flatten=False): + def get_link_frames(self, link_ids=None, flatten=False): """ Return the link frame position and orientation (expressed in the world space). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the frame position of all + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the frame position of all links associated to actuated joints. flatten (bool): if True, it will return a 1D array instead of a 2D array Returns: if 1 link: - float[3]: the link frame position in the world space - quaternion: Cartesian orientation of the link frame (w,x,y,z) + np.float[3]: the link frame position in the world space + np.float[4]: Cartesian orientation of the link frame [x,y,z,w] if multiple links: - float[Nx3], float[N,3]: link frame position of each link in world space - float[Nx4], quaternion[N]: orientation of each link frame (w,x,y,z) + np.float[Nx3], np.float[N,3]: link frame position of each link in world space + np.float[Nx4], np.float[N,4]: orientation of each link frame [x,y,z,w] """ - return self.getLinkFrameWorldPositions(linkId, flatten), self.getLinkFrameWorldOrientations(linkId, flatten) + return self.get_link_frame_world_positions(link_ids, flatten), self.get_link_frame_world_orientations(link_ids, + flatten) - def getLinkFrameWorldPositions(self, linkId=None, flatten=False): + def get_link_frame_world_positions(self, link_ids=None, flatten=False): """ Return the frame position (in the Cartesian world space coordinates) of the given link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the frame position of all + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the frame position of all links associated to actuated joints. flatten (bool): if True, it will return a 1D array instead of a 2D array Returns: if 1 link: - float[3]: the link frame position in the world space + np.float[3]: the link frame position in the world space if multiple links: - float[Nx3], float[N,3]: link frame position of each link in world space + np.float[Nx3], np.float[N,3]: link frame position of each link in world space """ - if isinstance(linkId, int): - return np.array(self.sim.getLinkState(self.id, linkId)[4]) - if linkId is None: - linkId = self.joints - pos = np.array([self.sim.getLinkState(self.id, link)[4] for link in linkId]) + if isinstance(link_ids, int): + return np.array(self.sim.get_link_state(self.id, link_ids)[4]) + if link_ids is None: + link_ids = self.joints + pos = np.array([self.sim.get_link_state(self.id, link)[4] for link in link_ids]) if flatten: - return pos.reshape(-1) # 1d array + return pos.reshape(-1) # 1D array return pos # 2D array - def getLinkFrameWorldOrientations(self, linkId=None, flatten=False): + def get_link_frame_world_orientations(self, link_ids=None, flatten=False): """ Return the frame orientation (in the Cartesian world space) of the given link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the frame orientation of + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the frame orientation of all links associated to actuated joints. flatten (bool): if True, it will return a 1D array of float numbers instead of an array of quaternion Returns: if 1 link: - quaternion: Cartesian orientation of the link frame (w,x,y,z) + np.float[4]: Cartesian orientation of the link frame [x,y,z,w] if multiple links: - float[Nx4], quaternion[N]: orientation of each link frame (w,x,y,z) + np.float[Nx4], np.float[N,4]: orientation of each link frame [x,y,z,w] """ - if isinstance(linkId, int): - return self._convert_to_quat(self.sim.getLinkState(self.id, linkId)[5]) - if linkId is None: - linkId = self.joints - orientation = np.array([self.angular_converter.convertTo(self.sim.getLinkState(self.id, link)[5]) - for link in linkId]) + if isinstance(link_ids, int): + return self.sim.get_link_state(self.id, link_ids)[5] + if link_ids is None: + link_ids = self.joints + orientation = np.array([self.sim.get_link_state(self.id, link)[5] for link in link_ids]) if flatten: - return quaternion.as_float_array(orientation).reshape(-1) - return orientation # array of quaternions + return orientation.reshape(-1) # 1D array + return orientation # 2D array - def getLinkWorldPositions(self, linkId=None, flatten=True): + def get_link_world_positions(self, link_ids=None, flatten=True): """ Return the CoM position (in the Cartesian world space coordinates) of the given link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the position of all links + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the position of all links associated to actuated joints. flatten (bool): if True, it will return a 1D array instead of a 2D array Returns: if 1 link: - float[3]: the link CoM position in the world space + np.float[3]: the link CoM position in the world space if multiple links: - float[Nx3], float[N,3]: CoM position of each link in world space + np.float[Nx3], np.float[N,3]: CoM position of each link in world space """ - if isinstance(linkId, int): - if linkId == -1: - return self.getBasePosition() - return np.array(self.sim.getLinkState(self.id, linkId)[0]) - if linkId is None: - linkId = self.joints - pos = np.array([self.sim.getLinkState(self.id, link)[0] for link in linkId]) + if isinstance(link_ids, int): + if link_ids == -1: + return self.get_base_position() + return np.array(self.sim.get_link_state(self.id, link_ids)[0]) + if link_ids is None: + link_ids = self.joints + pos = np.array([self.sim.get_link_state(self.id, link)[0] for link in link_ids]) if flatten: - return pos.reshape(-1) # 1d array - return pos # 2D array + return pos.reshape(-1) # 1D array + return pos # 2D array - def getLinkPositions(self, linkId=None, wrtLinkId=None, flatten=True): + def get_link_positions(self, link_ids=None, wrt_link_id=None, flatten=True): """ Return the link CoM position wrt the position of another link. By default, it is the base. Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the position of all links + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the position of all links associated to actuated joints. - wrtLinkId (int, int[N], None): the other link id(s). If None, returns the position wrt to the base. + wrt_link_id (int, int[N], None): the other link id(s). If None, returns the position wrt to the base. flatten (bool): if True, it will return a 1D array instead of a 2D array Returns: if 1 link: - float[3]: the link CoM position + np.float[3]: the link CoM position if multiple links: - float[Nx3], float[N,3]: CoM position of each link + np.float[Nx3], np.float[N,3]: CoM position of each link """ - p1 = self.getLinkWorldPositions(linkId, flatten=False) - p0 = self.getBasePosition() if wrtLinkId is None or wrtLinkId == -1 \ - else self.getLinkWorldPositions(wrtLinkId, flatten=False) + p1 = self.get_link_world_positions(link_ids, flatten=False) + p0 = self.get_base_position() if wrt_link_id is None or wrt_link_id == -1 \ + else self.get_link_world_positions(wrt_link_id, flatten=False) p = (p1 - p0) if flatten: return p.reshape(-1) return p - def getLinkWorldOrientations(self, linkId=None, flatten=True): + def get_link_world_orientations(self, link_ids=None, flatten=True): """ Return the CoM orientation (in the Cartesian world space) of the given link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the orientation of all links + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the orientation of all links associated to actuated joints. flatten (bool): if True, it will return a 1D array of float numbers instead of an array of quaternion Returns: if 1 link: - quaternion: Cartesian orientation of the link CoM (w,x,y,z) + np.float[4]: Cartesian orientation of the link CoM [x,y,z,w] if multiple links: - float[Nx4], quaternion[N]: CoM orientation of each link (w,x,y,z) + float[Nx4], np.float[N,4]: CoM orientation of each link [x,y,z,w] """ - if isinstance(linkId, int): - return self._convert_to_quat(self.sim.getLinkState(self.id, linkId)[1]) - if linkId is None: - linkId = self.joints - orientation = np.array([self.angular_converter.convertTo(self.sim.getLinkState(self.id, link)[1]) - for link in linkId]) + if isinstance(link_ids, int): + return self.sim.get_link_state(self.id, link_ids)[1] + if link_ids is None: + link_ids = self.joints + orientation = np.array([self.sim.get_link_state(self.id, link)[1] for link in link_ids]) if flatten: - return quaternion.as_float_array(orientation).reshape(-1) - return orientation # array of quaternions + return orientation.reshape(-1) + return orientation # 2D array - def getLinkOrientations(self, linkId=None, wrtLinkId=None, flatten=True): + def get_link_orientations(self, link_ids=None, wrt_link_id=None, flatten=True): """ Return the link CoM orientation wrt the orientation of another link. By default, it is the base. Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the orientation of all links + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the orientation of all links associated to actuated joints. - wrtLinkId (int, int[N], None): the other link id(s). If None, returns the orientation wrt to the base. + wrt_link_id (int, int[N], None): the other link id(s). If None, returns the orientation wrt to the base. flatten (bool): if True, it will return a 1D array instead of a 2D array Returns: if 1 link: - quaternion: Cartesian orientation of the link CoM (w,x,y,z) + np.float[4]: Cartesian orientation of the link CoM [x,y,z,w] if multiple links: - float[Nx4], quaternion[N]: CoM orientation of each link (w,x,y,z) + float[Nx4], np.float[N,4]: CoM orientation of each link [x,y,z,w] """ - q1 = self.getLinkWorldOrientations(linkId) - if wrtLinkId is None or wrtLinkId == -1: - q0 = self.getBaseOrientation().inverse() + q1 = self.get_link_world_orientations(link_ids) + if wrt_link_id is None or wrt_link_id == -1: + q0 = get_quaternion_inverse(self.get_base_orientation()) else: - if isinstance(wrtLinkId, int): - q0 = self.getLinkWorldOrientations(wrtLinkId).inverse() + if isinstance(wrt_link_id, int): + q0 = get_quaternion_inverse(self.get_link_world_orientations(wrt_link_id)) else: - q0 = np.array([self.getLinkWorldOrientations(link).inverse() for link in wrtLinkId]) + q0 = np.array([get_quaternion_inverse(self.get_link_world_orientations(link)) for link in wrt_link_id]) - q = q0 * q1 + q = get_quaternion_product(q0, q1) if flatten: - quaternion.as_float_array(q).reshape(-1) + q.reshape(-1) return q - def getLinkWorldLinearVelocities(self, linkId=None, flatten=True): + def get_link_world_linear_velocities(self, link_ids=None, flatten=True): """ Return the linear velocity of the link(s) expressed in the Cartesian world space coordinates. Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the linear velocities of + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the linear velocities of all links associated to actuated joints. flatten (bool): if True, it will return a 1D array instead of a 2D array Returns: if 1 link: - float[3]: linear velocity of the link in the Cartesian world space + np.float[3]: linear velocity of the link in the Cartesian world space if multiple links: - float[Nx3], float[N,3]: linear velocity of each link + np.float[Nx3], np.float[N,3]: linear velocity of each link """ - if isinstance(linkId, int): - return np.array(self.sim.getLinkState(self.id, linkId, computeLinkVelocity=1)[6]) - if linkId is None: - linkId = self.joints - vel = np.array([self.sim.getLinkState(self.id, link, computeLinkVelocity=1)[6] for link in linkId]) + if isinstance(link_ids, int): + return np.array(self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[6]) + if link_ids is None: + link_ids = self.joints + vel = np.array([self.sim.get_link_state(self.id, link, compute_velocity=True)[6] for link in link_ids]) if flatten: - return vel.reshape(-1) # 1d array + return vel.reshape(-1) # 1D array return vel # 2D array - def getLinkWorldAngularVelocities(self, linkId=None, flatten=True): + def get_link_world_angular_velocities(self, link_ids=None, flatten=True): """ Return the angular velocity of the link(s) in the Cartesian world space coordinates. Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the angular velocities of + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the angular velocities of all links associated to actuated joints. flatten (bool): if True, it will return a 1D array instead of a 2D array Returns: if 1 link: - float[3]: angular velocity of the link in the Cartesian world space + np.float[3]: angular velocity of the link in the Cartesian world space if multiple links: - float[Nx3], float[N,3]: angular velocity of each link + np.float[Nx3], np.float[N,3]: angular velocity of each link """ - if isinstance(linkId, int): - return np.array(self.sim.getLinkState(self.id, linkId, computeLinkVelocity=1)[7]) - if linkId is None: - linkId = self.joints - vel = np.array([self.sim.getLinkState(self.id, link, computeLinkVelocity=1)[7] for link in linkId]) + if isinstance(link_ids, int): + return np.array(self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[7]) + if link_ids is None: + link_ids = self.joints + vel = np.array([self.sim.get_link_state(self.id, link, compute_velocity=True)[7] for link in link_ids]) if flatten: return vel.reshape(-1) # 1d array return vel # 2D array - def getLinkWorldVelocities(self, linkId=None, flatten=True): + def get_link_world_velocities(self, link_ids=None, flatten=True): """ Return the linear and angular velocities (expressed in the Cartesian world space coordinates) for the given link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the linear and angular + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the linear and angular velocities of all links associated to actuated joints. flatten (bool): if True, it will return a 1D array instead of a 2D array @@ -1676,102 +1386,102 @@ class Robot(object): if multiple links: float[Nx6], float[N,6]: linear and angular velocity of each link """ - if isinstance(linkId, int): - lin_vel, ang_vel = self.sim.getLinkState(self.id, linkId, computeLinkVelocity=1)[6:8] + if isinstance(link_ids, int): + lin_vel, ang_vel = self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[6:8] return np.array(lin_vel + ang_vel) - if linkId is None: - linkId = self.joints + if link_ids is None: + link_ids = self.joints vel = [] - for link in linkId: - lin_vel, ang_vel = self.sim.getLinkState(self.id, link, computeLinkVelocity=1)[6:8] + for link in link_ids: + lin_vel, ang_vel = self.sim.get_link_state(self.id, link, compute_velocity=True)[6:8] vel.append(lin_vel + ang_vel) vel = np.array(vel) if flatten: return vel.reshape(-1) # 1d array return vel # 2D array - def getLinkLinearVelocities(self, linkId=None, wrtLinkId=None, flatten=True): + def get_link_linear_velocities(self, link_ids=None, wrt_link_id=None, flatten=True): """ Return the linear velocity of the given link(s) wrt the other specified link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the linear velocity of + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the linear velocity of all links associated to actuated joints. - wrtLinkId (int, int[N], None): the other link id(s). If None, returns the linear velocity wrt to the base. + wrt_link_id (int, int[N], None): the other link id(s). If None, returns the linear velocity wrt to the base. flatten (bool): if True, it will return a 1D array instead of a 2D array Returns: if 1 link: - float[3]: the linear velocity of the given link wrt to the other link + np.float[3]: the linear velocity of the given link wrt to the other link if multiple links: - float[Nx3], float[N,3]: linear velocity of each link wrt to the other link(s) + np.float[Nx3], np.float[N,3]: linear velocity of each link wrt to the other link(s) """ - v1 = self.getLinkWorldLinearVelocities(linkId, flatten=False) - v0 = self.getBaseLinearVelocity() if wrtLinkId is None or wrtLinkId == -1 \ - else self.getLinkWorldLinearVelocities(wrtLinkId, flatten=False) + v1 = self.get_link_world_linear_velocities(link_ids, flatten=False) + v0 = self.get_base_linear_velocity() if wrt_link_id is None or wrt_link_id == -1 \ + else self.get_link_world_linear_velocities(wrt_link_id, flatten=False) v = (v1 - v0) if flatten: return v.reshape(-1) return v - def getLinkAngularVelocities(self, linkId=None, wrtLinkId=None, flatten=True): + def get_link_angular_velocities(self, link_ids=None, wrt_link_id=None, flatten=True): """ Return the angular velocity of the given link(s) wrt to the other specified link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the angular velocity of + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the angular velocity of all links associated to actuated joints. - wrtLinkId (int, int[N], None): the other link id(s). If None, returns the angular velocity wrt to the base. + wrt_link_id (int, int[N], None): the other link id(s). If None, returns the angular velocity wrt to the base. flatten (bool): if True, it will return a 1D array instead of a 2D array Returns: if 1 link: - float[3]: the angular velocity of the given link wrt to the other link + np.float[3]: the angular velocity of the given link wrt to the other link if multiple links: - float[Nx3], float[N,3]: angular velocity of each link wrt to the other link(s) + np.float[Nx3], np.float[N,3]: angular velocity of each link wrt to the other link(s) """ - w1 = self.getLinkWorldAngularVelocities(linkId, flatten=False) - w0 = self.getBaseAngularVelocity() if wrtLinkId is None or wrtLinkId == -1 \ - else self.getLinkWorldAngularVelocities(wrtLinkId, flatten=False) + w1 = self.get_link_world_angular_velocities(link_ids, flatten=False) + w0 = self.get_base_angular_velocity() if wrt_link_id is None or wrt_link_id == -1 \ + else self.get_link_world_angular_velocities(wrt_link_id, flatten=False) w = (w1 - w0) if flatten: return w.reshape(-1) return w - def getLinkVelocities(self, linkId=None, wrtLinkId=None, flatten=True): + def get_link_velocities(self, link_ids=None, wrt_link_id=None, flatten=True): """ Return the linear and angular velocity of the given link(s) wrt to the other specified link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get the angular velocity of + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the angular velocity of all links associated to actuated joints. - wrtLinkId (int, int[N], None): the other link id(s). If None, returns the angular velocity wrt to the base. + wrt_link_id (int, int[N], None): the other link id(s). If None, returns the angular velocity wrt to the base. flatten (bool): if True, it will return a 1D array instead of a 2D array Returns: if 1 link: - float[6]: the linear and angular velocity of the given link wrt to the other link + np.float[6]: the linear and angular velocity of the given link wrt to the other link if multiple links: - float[Nx6], float[N,6]: linear and angular velocity of each link wrt to the other link(s) + np.float[Nx6], np.float[N,6]: linear and angular velocity of each link wrt to the other link(s) """ - v1 = self.getLinkWorldVelocities(linkId, flatten=False) - v0 = self.getBaseVelocity() if wrtLinkId is None or wrtLinkId == -1 \ - else self.getLinkWorldVelocities(wrtLinkId, flatten=False) + v1 = self.get_link_world_velocities(link_ids, flatten=False) + v0 = self.get_base_velocity() if wrt_link_id is None or wrt_link_id == -1 \ + else self.get_link_world_velocities(wrt_link_id, flatten=False) v = (v1 - v0) if flatten: return v.reshape(-1) return v - def getLinkLinearAccelerations(self, linkId=None): - raise NotImplementedError + def get_link_linear_accelerations(self, link_ids=None): + pass - def getLinkAngularAccelerations(self, linkId=None): - raise NotImplementedError + def get_link_angular_accelerations(self, link_ids=None): + pass - def getLinkAccelerations(self, linkId=None): - raise NotImplementedError + def get_link_accelerations(self, link_ids=None): + pass - def getLinkContacts(self, linkId): + def get_link_contacts(self, link_ids): """ Check if the given link(s) is/are in contact with something in the environment, and return all the contact points involving the given robot link(s). @@ -1780,7 +1490,7 @@ class Robot(object): is a sensor attached to it. However, this can be useful in simulation to optimize, for instance, trajectories. Args: - linkId (int, int[N]): link id, or list of desired link ids. + link_ids (int, int[N]): link id, or list of desired link ids. Returns: if 1 link: @@ -1790,18 +1500,18 @@ class Robot(object): int: unique id of body B int: link index of body A (-1 for base, this should be the same as the given link) int: link index of body B (-1 for base) - float[3]: contact position on A (in Cartesian world coordinates) - float[3]: contact position on B (in Cartesian world coordinates) - float[3]: contact normal on B pointing towards A + np.float[3]: contact position on A (in Cartesian world coordinates) + np.float[3]: contact position on B (in Cartesian world coordinates) + np.float[3]: contact normal on B pointing towards A float: contact distance (positive for separation and negative for penetration) float: normal force applied during the last simulation step if multiple links: list of above """ - if isinstance(linkId, int): - return self.sim.getContactPoints(bodyA=self.id, linkIndexA=linkId) - return [self.sim.getContactPoints(bodyA=self.id, linkIndexA=link) for link in linkId] + if isinstance(link_ids, int): + return self.sim.get_contact_points(body1=self.id, link1_id=link_ids) + return [self.sim.get_contact_points(body1=self.id, link1_id=link) for link in link_ids] - def setLinkPositions(self, linkId, position, orientation=None): + def set_link_positions(self, link_ids, position, orientation=None): """ Set the position(s) of the given link(s) using inverse kinematics (IK). @@ -1809,9 +1519,9 @@ class Robot(object): the position of multiple links that share some joints, you will get positions that are inconsistents. Args: - linkId (int, int[N]): link id, or list of desired link ids. - position (float[3], [float[3]], float[N,3]): - orientation (float[4], [float[4]], float[N,4]): + link_ids (int, int[N]): link id, or list of desired link ids. + position (np.float[3], [float[3]], float[N,3]): + orientation (np.float[4], [float[4]], float[N,4]): """ pass @@ -1819,30 +1529,8 @@ class Robot(object): # End-Effectors # # same interface than Links (but easier to manipulate) # ################# - def _setEndEffectors(self): - """ - Set automatically the end-effector ids and names based on the URDF. Here, all the leaves of the robot - kinematic tree will be considered as end-effectors. Thus, use it with caution. - """ - if len(self.end_effectors) == 0: - end_effectors = {} - - # go through all the joints/links - for joint in range(self.getNumberOfJoints()): - # get useful information from current joint/link - info = self.sim.getJointInfo(self.id, joint) - parentIdx, linkName = info[-1], info[-5] - - # add this link in the end-effectors dict - end_effectors[joint] = linkName - - # remove parent index from the end-effectors dict if present - end_effectors.pop(parentIdx, None) - - self.end_effectors = end_effectors.keys() - self.end_effector_names = {name: idx for idx, name in end_effectors.items()} - - def getNumberOfEndEffectors(self): + @property + def num_end_effectors(self): """ Return the number of end-effectors. @@ -1851,14 +1539,37 @@ class Robot(object): """ return len(self.end_effectors) - def getEndEffectorIds(self, endEffector=None): + def _set_end_effectors(self): + """ + Set automatically the end-effector ids and names based on the URDF. Here, all the leaves of the robot + kinematic tree will be considered as end-effectors. Thus, use it with caution. + """ + if len(self.end_effectors) == 0: + end_effectors = {} + + # go through all the joints/links + for joint in range(self.num_joints): + # get useful information from current joint/link + info = self.sim.get_joint_info(self.id, joint) + parent_idx, link_name = info[-1], info[-5] + + # add this link in the end-effectors dict + end_effectors[joint] = link_name + + # remove parent index from the end-effectors dict if present + end_effectors.pop(parent_idx, None) + + self.end_effectors = end_effectors.keys() + self.end_effector_names = {name: idx for idx, name in end_effectors.items()} + + def get_end_effector_ids(self, end_effector=None): """ Get the end effector ids from the name(s) or index(ices). Note that the end-effector id is unique and goes from 0 to the total number of end-effectors. Args: - endEffector (str, int, list of str/int, None): if str, it will get the end-effector id associated to the + end_effector (str, int, list of str/int, None): if str, it will get the end-effector id associated to the given name. If int, it will get the end-effector id associated to the given q index. If it is a list of str and/or int, it will get the corresponding end-effector ids. If None, it will return all the end-effector ids. @@ -1869,10 +1580,10 @@ class Robot(object): if multiple end-effectors: int[N]: end-effector ids """ - if endEffector is None: + if end_effector is None: return self.end_effectors - def getIndex(link): + def get_index(link): if isinstance(link, str): return self.end_effector_names[link] elif isinstance(link, int): @@ -1881,302 +1592,41 @@ class Robot(object): raise TypeError("Incorrect type") # list of links - if isinstance(endEffector, collections.Iterable) and not isinstance(endEffector, str): - return [getIndex(link) for link in endEffector] + if isinstance(end_effector, collections.Iterable) and not isinstance(end_effector, str): + return [get_index(link) for link in end_effector] # one link - return getIndex(endEffector) - - def getEndEffectorNames(self, endEffectorId=None): - """ - Return the name of the given end-effector(s). - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the name of all end-effectors. - - Returns: - if 1 end-effector: - str: end-effector name - if multiple end-effectors: - str[N]: name of each end-effector - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkNames(endEffectorId) - - def getEndEffectorWorldPositions(self, endEffectorId=None, flatten=True): - """ - Return the world position of the given end-effector(s). - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the position of all end-effectors. - flatten (bool): if True, it will return a 1D array instead of a 2D array - - Returns: - if 1 end-effector: - float[3]: world position of the end-effector - if multiple end-effectors: - float[Nx3], float[N,3]: world position of each end-effector - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkWorldPositions(endEffectorId, flatten) - - def getEndEffectorPositions(self, endEffectorId=None, wrtLinkId=None, flatten=True): - """ - Return the position of the end-effector(s) wrt the position of (an)other link(s). By default, it is the base. - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the position of all end-effectors. - wrtLinkId (int, int[N], None): the other link id(s). If None, returns the position wrt to the base. - flatten (bool): if True, it will return a 1D array instead of a 2D array - - Returns: - if 1 end-effector: - float[3]: position of the end-effector wrt the link - if multiple end-effectors: - float[Nx3], float[N,3]: position of each end-effector wrt the link - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkPositions(endEffectorId, wrtLinkId, flatten) - - def getEndEffectorWorldOrientations(self, endEffectorId=None, flatten=True): - """ - Return the end-effector orientation (in the Cartesian world space). - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the orientation of all end-effectors. - flatten (bool): if True, it will return a 1D array of float numbers instead of an array of quaternion - - Returns: - if 1 end-effector: - quaternion: orientation of the end-effector (w,x,y,z) - if multiple end-effectors: - float[Nx4], quaternion[N]: orientation of each end-effector (w,x,y,z) - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkWorldOrientations(endEffectorId, flatten) - - def getEndEffectorOrientations(self, endEffectorId=None, wrtLinkId=None, flatten=True): - """ - Return the end-effector orientation wrt to specified link(s). By default, it will be wrt to the base - orientation. - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the orientation of all end-effectors. - wrtLinkId (int, int[N], None): the other link id(s). If None, returns the orientation wrt to the base. - flatten (bool): if True, it will return a 1D array of float numbers instead of an array of quaternion - - Returns: - if 1 end-effector: - quaternion: orientation of the end-effector (w,x,y,z) - if multiple end-effectors: - float[Nx4], quaternion[N]: orientation of each end-effector (w,x,y,z) - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkOrientations(endEffectorId, wrtLinkId, flatten) - - def getEndEffectorWorldVelocities(self, endEffectorId=None, flatten=True): - """ - Return the linear and angular velocities (expressed in the Cartesian world space coordinates) for the given - end-effector(s). - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the velocities of all end-effectors. - flatten (bool): if True, it will return a 1D array instead of a 2D array - - Returns: - if 1 end-effector: - float[6]: linear and angular velocity of the end-effector in the Cartesian world space - if multiple end-effectors: - float[Nx6], float[N,6]: linear and angular velocity of each end-effector - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkWorldVelocities(endEffectorId, flatten) - - def getEndEffectorVelocities(self, endEffectorId=None, wrtLinkId=None, flatten=True): - """ - Return the linear and angular velocities (expressed in the Cartesian world space coordinates) for the given - end-effector(s) wrt to specified link(s). By default, it is the base. - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the velocities of all end-effectors. - wrtLinkId (int, int[N], None): the other link id(s). If None, returns the velocities wrt to the base. - flatten (bool): if True, it will return a 1D array instead of a 2D array - - Returns: - if 1 end-effector: - float[6]: linear and angular velocity of the end-effector - if multiple end-effectors: - float[Nx6], float[N,6]: linear and angular velocity of each end-effector - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkVelocities(endEffectorId, wrtLinkId, flatten) - - def getEndEffectorWorldLinearVelocities(self, endEffectorId=None, flatten=True): - """ - Return the linear velocities (expressed in the Cartesian world space coordinates) for the given - end-effector(s). - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the linear velocities of all end-effectors. - flatten (bool): if True, it will return a 1D array instead of a 2D array - - Returns: - if 1 end-effector: - float[3]: linear velocity of the end-effector in the Cartesian world space - if multiple end-effectors: - float[Nx3], float[N,3]: linear velocity of each end-effector - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkWorldLinearVelocities(endEffectorId, flatten) - - def getEndEffectorLinearVelocities(self, endEffectorId=None, wrtLinkId=None, flatten=True): - """ - Return the linear velocities (expressed in the Cartesian world space coordinates) for the given - end-effector(s) wrt to specified link(s). By default, it is the base. - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the linear velocities of all end-effectors. - wrtLinkId (int, int[N], None): the other link id(s). If None, returns the linear velocities wrt to - the base. - flatten (bool): if True, it will return a 1D array instead of a 2D array - - Returns: - if 1 end-effector: - float[3]: linear velocity of the end-effector - if multiple end-effectors: - float[Nx3], float[N,3]: linear velocity of each end-effector - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkLinearVelocities(endEffectorId, wrtLinkId, flatten) - - def getEndEffectorWorldAngularVelocities(self, endEffectorId=None, flatten=True): - """ - Return the angular velocities (expressed in the Cartesian world space coordinates) for the given - end-effector(s). - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the angular velocities of all end-effectors. - flatten (bool): if True, it will return a 1D array instead of a 2D array - - Returns: - if 1 end-effector: - float[3]: angular velocity of the end-effector in the Cartesian world space - if multiple end-effectors: - float[Nx3], float[N,3]: angular velocity of each end-effector - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkWorldAngularVelocities(endEffectorId, flatten) - - def getEndEffectorAngularVelocities(self, endEffectorId=None, wrtLinkId=None, flatten=True): - """ - Return the angular velocities (expressed in the Cartesian world space coordinates) for the given - end-effector(s) wrt to specified link(s). By default, it is the base. - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the angular velocities of all end-effectors. - wrtLinkId (int, int[N], None): the other link id(s). If None, returns the angular velocities wrt to - the base. - flatten (bool): if True, it will return a 1D array instead of a 2D array - - Returns: - if 1 end-effector: - float[3]: angular velocity of the end-effector - if multiple end-effectors: - float[Nx3], float[N,3]: angular velocity of each end-effector - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkAngularVelocities(endEffectorId, wrtLinkId, flatten) - - def getEndEffectorAccelerations(self, endEffectorId=None, flatten=True): - raise NotImplementedError - - def getEndEffectorForce(self, endEffectorId=None, flatten=True): - raise NotImplementedError - - def getEndEffectorContacts(self, endEffectorId=None): - """ - Check if the given end-effector(s) is/are in contact with something in the environment, and return all the - contact points involving the given robot end-effector(s). - - Warnings: note that in reality, you can't know if your end-effector(s) is/are in contact with an object unless - there is a sensor attached to it. However, this can be useful in simulation to optimize, for instance, - trajectories. - - Args: - endEffectorId (int, int[N], None): end-effector id, or list of desired end-effector ids. - If None, get the contacts of all end-effectors. - - Returns: - if 1 end-effector: - list: list of contact points where each contact point has: - int: contact flag - int: unique id of body A (this should be the robot id) - int: unique id of body B - int: link index of body A (-1 for base, this should be the same as the given link) - int: link index of body B (-1 for base) - float[3]: contact position on A (in Cartesian world coordinates) - float[3]: contact position on B (in Cartesian world coordinates) - float[3]: contact normal on B pointing towards A - float: contact distance (positive for separation and negative for penetration) - float: normal force applied during the last simulation step - if multiple end-effectors: list of above - """ - if endEffectorId is None: - endEffectorId = self.end_effectors - return self.getLinkContacts(endEffectorId) + return get_index(end_effector) ############## # Transforms # ############## - def getHomogeneousTransform(self, position, orientation): + def get_homogeneous_transform(self, position, orientation): """ Return the Homogeneous transform matrix given the position vector and the orientation. Args: - position (float[3]): position vector - orientation (np.quaternion, float[4], float[3,3], float[3]): orientation + position (np.float[3]): position vector + orientation (np.float[4], np.float[3,3], np.float[3]): orientation Returns: - float[4,4]: homogeneous matrix + np.float[4,4]: homogeneous matrix """ if isinstance(orientation, quaternion.quaternion): R = quaternion.as_rotation_matrix(orientation) else: orientation = np.array(orientation) - if orientation.shape == (3,): # RPY Euler angles - R = self.sim.getMatrixFromQuaternion(self.sim.getQuaternionFromEuler(orientation)) - R = np.array(R).reshape(3,3) - elif orientation.shape == (4,): # quaternion in the form (x,y,z,w) - R = np.array(self.sim.getMatrixFromQuaternion(orientation)).reshape(3,3) - elif orientation.shape == (3,3): # Rotation matrix + if orientation.shape == (3,): # RPY Euler angles + R = get_matrix_from_rpy(orientation) + elif orientation.shape == (4,): # quaternion in the form [x,y,z,w] + R = get_matrix_from_quaternion(orientation) + elif orientation.shape == (3, 3): # Rotation matrix R = orientation else: raise ValueError("Expecting a quaternion, RPY Euler angles, or rotation matrix") - H = np.vstack((np.hstack((R, position.reshape(-1,1))), np.array([[0,0,0,1]]))) + H = np.vstack((np.hstack((R, position.reshape(-1, 1))), np.array([[0, 0, 0, 1]]))) return H ############## @@ -2184,7 +1634,7 @@ class Robot(object): ############## # TODO: allow to slice the Jacobian to only get what interests the user - def calculateJacobian(self, linkId, q=None, localPosition=None): + def get_jacobian(self, link_id, q=None, local_position=None): """ Return the full geometric Jacobian matrix :math:`J(q) = [J_{lin}(q), J_{ang}(q)]^T`, such that: @@ -2196,38 +1646,35 @@ class Robot(object): link DoFs (at the beginning). If it is a fixed base, it will only have columns associated with the joints. Args: - linkId (int): linkId - q (float[N]): joint positions of size N, where N is the number of DoFs. If None, it will compute q based + link_id (int): link id. + q (np.float[N]): joint positions of size N, where N is the number of DoFs. If None, it will compute q based on the current joint positions. - localPosition: the point on the specified link to compute the Jacobian (in link local coordinates around + local_position: the point on the specified link to compute the Jacobian (in link local coordinates around its center of mass). If None, it will use the CoM position (in the link frame). Returns: - float[6,N], float[6,(6+N)]: full geometric (linear and angular) Jacobian matrix. The number of columns + np.float[6,N], np.float[6,(6+N)]: full geometric (linear and angular) Jacobian matrix. The number of columns depends if the base is fixed or floating. """ if q is None: - q = self.getJointPositions() + q = self.get_joint_positions() else: if len(q) != len(self.joints): raise ValueError("The length of q ({}) is different from the number of DoFs" " ({}).".format(len(q), len(self.joints))) if isinstance(q, np.ndarray): - q = q.tolist() # Note that q has to be a list; it doesn't work if numpy array in Pybullet + q = q.tolist() # Note that q has to be a list; it doesn't work if numpy array in Pybullet dq = [0]*len(self.joints) # specify point on the link - if localPosition is None: - localPosition = self.sim.getLinkState(self.id, linkId)[2] # Link CoM position in the link frame + if local_position is None: + local_position = self.sim.get_link_state(self.id, link_id)[2] # Link CoM position in the link frame # calculate full jacobian - lin_jac, ang_jac = self.sim.calculateJacobian(self.id, linkId, localPosition=localPosition, - objPositions=q, objVelocities=dq, objAccelerations=dq) + return self.sim.calculate_jacobian(self.id, link_id, local_position=local_position, q=q, dq=dq, des_ddq=dq) - return np.vstack((lin_jac, ang_jac)) - - def calculateLinearJacobian(self, linkId, q=None, localPosition=None): + def get_linear_jacobian(self, link_id, q=None, local_position=None): """ Return the full linear (geometric) Jacobian matrix :math:`J_{lin}(q)`, such that: @@ -2239,19 +1686,19 @@ class Robot(object): link DoFs (at the beginning). If it is a fixed base, it will only have columns associated with the joints. Args: - linkId (int): link id - q (float[N]): joint positions of size N, where N is the number of DoFs. If None, it will compute q based + link_id (int): link id + q (np.float[N]): joint positions of size N, where N is the number of DoFs. If None, it will compute q based on the current joint positions. - localPosition: the point on the specified link to compute the Jacobian (in link local coordinates around + local_position: the point on the specified link to compute the Jacobian (in link local coordinates around its center of mass). If None, it will use the CoM position (in the link frame). Returns: - float[3,N], float[3,(6+N)]: full linear geometric Jacobian matrix. The number of columns depends if the - base is fixed or floating. + np.float[3,N], np.float[3,(6+N)]: full linear geometric Jacobian matrix. The number of columns depends if + the base is fixed or floating. """ - return self.calculateJacobian(linkId, q, localPosition)[:3] + return self.get_jacobian(link_id, q, local_position)[:3] - def calculateAngularJacobian(self, linkId, q=None, localPosition=None): + def get_angular_jacobian(self, link_id, q=None, local_position=None): """ Return the full angular (geometric) Jacobian matrix :math:`J_{ang}(q)`, such that: @@ -2263,43 +1710,37 @@ class Robot(object): link DoFs (at the beginning). If it is a fixed base, it will only have columns associated with the joints. Args: - linkId (int): link id - q (float[N]): joint positions of size N, where N is the number of DoFs. If None, it will compute q based + link_id (int): link id + q (np.float[N]): joint positions of size N, where N is the number of DoFs. If None, it will compute q based on the current joint positions. - localPosition: the point on the specified link to compute the Jacobian (in link local coordinates around + local_position: the point on the specified link to compute the Jacobian (in link local coordinates around its center of mass). If None, it will use the CoM position (in the link frame). Returns: - float[3,N], float[3,(6+N)]: full angular geometric Jacobian matrix. The number of columns depends if the - base is fixed or floating. + np.float[3,N], np.float[3,(6+N)]: full angular geometric Jacobian matrix. The number of columns depends if + the base is fixed or floating. """ - return self.calculateJacobian(linkId, q, localPosition)[3:] + return self.get_jacobian(link_id, q, local_position)[3:] - # aliases - getJacobian = calculateJacobian - getGeometricJacobian = getJacobian - getLinearJacobian = calculateLinearJacobian - getAngularJacobian = calculateAngularJacobian - - def getJacobianDerivativeRPYToAngularVelocity(self, rpyAngle): + def get_jacobian_derivative_rpy_to_angular_velocity(self, rpy_angle): """ Return the Jacobian that maps RPY angle rates to angular velocities, i.e. :math:`\omega = T(\phi) \dot{\phi}`. Warnings: :math:`T` is singular when the pitch angle :math:`\theta_p = \pm \frac{\pi}{2}` Args: - rpyAngle (float[3]): RPY Euler angles [rad] + rpy_angle (float[3]): RPY Euler angles [rad] Returns: float[3,3]: Jacobian matrix that maps RPY angle rates to angular velocities. """ - r,p,y = rpyAngle + r, p, y = rpy_angle T = np.array([[1., 0., np.sin(p)], [0., np.cos(r), -np.cos(p) * np.sin(r)], [0., np.sin(r), np.cos(p) * np.cos(r)]]) return T - def getJacobianDerivativeZYZToAngularVelocity(self, zyzAngle): + def get_jacobian_derivative_zyz_to_angular_velocity(self, zyzAngle): """ Return the Jacobian that maps ZYZ angle rates to angular velocities, i.e. :math:`\omega = T(\phi) \dot{\phi}`. @@ -2317,7 +1758,7 @@ class Robot(object): [1., 0., np.cos(y)]]) return T - def getAnalyticalJacobian(self, jacobian, rpyAngle): + def get_analytical_jacobian(self, jacobian, rpy_angle): """ Return the analytical Jacobian :math:`J_{a}(q) = [J_{lin}(q), J_{\phi}(q)]^T`, which respects: @@ -2345,19 +1786,19 @@ class Robot(object): Args: jacobian (float[6,N], float[6,6+N]): full geometric Jacobian. - rpyAngle (float[3]): RPY Euler angles + rpy_angle (float[3]): RPY Euler angles Returns: float[6,N], foat[6,(6+N)]: the full analytical Jacobian. The number of columns depends if the base is fixed or floating. """ - T = self.getJacobianDerivativeRPYToAngularVelocity(rpyAngle) + T = self.get_jacobian_derivative_rpy_to_angular_velocity(rpy_angle) Tinv = np.linalg.inv(T) - Ja = np.vstack((np.hstack((np.identity(3), np.zeros((3,3)))), - np.hstack((np.zeros((3,3)), Tinv)))).dot(jacobian) + Ja = np.vstack((np.hstack((np.identity(3), np.zeros((3, 3)))), + np.hstack((np.zeros((3, 3)), Tinv)))).dot(jacobian) return Ja - def getAngularVelocitiesFromDerivativeRPY(self, rpyAngle, dRPY): + def get_angular_velocities_from_derivative_rpy(self, rpy_angle, dRPY): """ Return the angular velocities :math:`\omega` from the derivative of RPY Euler angles \math:`\dot{\phi}`. These 2 quantities are related by the following equation: @@ -2377,16 +1818,16 @@ class Robot(object): Note that :math:`T` is singular when the pitch angle :math:`\theta_p = \pm \frac{\pi}{2}`. Args: - rpyAngle (float[3]): RPY Euler angles [rad] + rpy_angle (float[3]): RPY Euler angles [rad] dRPY (float[3]): time derivative of RPY Euler angles [rad/s] Returns: float[3]: angular velocities [rad/s] """ - T = self.getJacobianDerivativeRPYToAngularVelocity(rpyAngle) + T = self.get_jacobian_derivative_rpy_to_angular_velocity(rpy_angle) return T.dot(dRPY) - def getDerivativeRPYFromAngularVelocities(self, rpyAngle, angularVelocity): + def get_derivative_rpy_from_angular_velocities(self, rpy_angle, angular_velocity): """ Return the time derivative of RPY Euler angles :math:`\dot{\phi}` given the angular velocities :math:`\omega`. @@ -2396,8 +1837,8 @@ class Robot(object): corresponding angular velocities :math:`\omega` are not defined. Args: - rollPitchYaw (float[3]): RPY Euler angles [rad] - angularVelocity (float[3]): angular velocities [rad/s] + rpy_angle (float[3]): RPY Euler angles [rad] + angular_velocity (float[3]): angular velocities [rad/s] Returns: float[3]: time derivative of RPY Euler angles [rad/s] @@ -2405,11 +1846,11 @@ class Robot(object): Raises: LinAlgError: if singular configuration. """ - T = self.getJacobianDerivativeRPYToAngularVelocity(rpyAngle) + T = self.get_jacobian_derivative_rpy_to_angular_velocity(rpy_angle) Tinv = np.linalg.inv(T) - return Tinv.dot(angularVelocity) + return Tinv.dot(angular_velocity) - def getJJT(self, jacobian): + def get_JJT(self, jacobian): """ Given the Jacobian, it returns :math:`JJ^T`. This relation is used in many places in robotics. @@ -2421,7 +1862,7 @@ class Robot(object): """ return jacobian.dot(jacobian.T) - def getDampedLeastSquaresInverse(self, jacobian, dampingFactor=0.01): + def get_damped_least_squares_inverse(self, jacobian, damping_factor=0.01): """ Return the damped least-squares (DLS) inverse, given by: @@ -2432,18 +1873,18 @@ class Robot(object): Args: jacobian (float[D,N]): Jacobian matrix - dampingFactor (float): damping factor + damping_factor (float): damping factor Returns: float[N,D]: DLS inverse matrix """ - J, k = jacobian, dampingFactor + J, k = jacobian, damping_factor return (J.T).dot(np.linalg.inv(J.dot(J.T) + k**2 * np.identity(J.shape[0]))) # alias - getDLSInverse = getDampedLeastSquaresInverse + getDLSInverse = get_damped_least_squares_inverse - def getPinvJacobian(self, jacobian): + def get_pinv_jacobian(self, jacobian): """ Return the right pseudo-inverse of the jacobian, i.e. :math:`J^\dagger = J^T(JJ^T)^{-1}`. @@ -2455,7 +1896,7 @@ class Robot(object): """ return np.linalg.pinv(jacobian) - def getNullSpaceProjector(self, jacobian): + def get_null_space_projector(self, jacobian): """ The null space projector :math:`P` is the matrix that projects any vectors to the null space of :math:`J`. This is given by: :math:`P = (I - J^\dagger J)`, where :math:`J^\dagger = J^T(JJ^T)^{-1}` is the right @@ -2469,11 +1910,11 @@ class Robot(object): float[N,N]: null space projector matrix """ J = jacobian - JJT = self.getJJT(jacobian) + JJT = self.get_JJT(jacobian) I = np.identity(J.shape[1]) - return (I - self.getPinvJacobian(J=J).dot(J)) + return I - self.get_pinv_jacobian(J=J).dot(J) - def computeManipulabilityMeasure(self, jacobian): + def compute_manipulability_measure(self, jacobian): """ Compute the manipulability measure `w(q) = sqrt( det(J(q)J(q)^T) )`. This is useful to get a general sense about the manipulation ability of the manipulator. This term, for instance, vanishes at singular @@ -2488,9 +1929,9 @@ class Robot(object): References: [1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010, chap 3.5 and 3.9 """ - return np.sqrt(np.linalg.det(self.getJJT(jacobian))) + return np.sqrt(np.linalg.det(self.get_JJT(jacobian))) - def isInSingularConfiguration(self, jacobian): + def in_singular_configuration(self, jacobian): """ Return True if we are in a singular configuration. @@ -2511,10 +1952,10 @@ class Robot(object): # TODO: define close to singular configuration using SVD J = jacobian m = np.min(J.shape) - r = np.linalg.matrix_rank(J) # this uses SVD to compute the rank - return (r < m) + r = np.linalg.matrix_rank(J) # this uses SVD to compute the rank + return r < m - def getJointVelocitiesFromCartesianVelocities(self, jacobian, velocity): + def get_joint_velocities_from_cartesian_velocities(self, jacobian, velocity): """ Return the joint velocities :math:`\dot{q}` from the cartesian velocities :math:`v`. @@ -2529,10 +1970,10 @@ class Robot(object): Returns: float[N]: joint velocities """ - Jpinv = self.getPinvJacobian(jacobian) + Jpinv = self.get_pinv_jacobian(jacobian) return Jpinv.dot(velocity) - def getCartesianVelocitiesFromJointVelocities(self, jacobian, dq): + def get_cartesian_velocities_from_joint_velocities(self, jacobian, dq): """ Return the Cartesian velocities :math:`v = [\dot{p}, \omega]^T` where :math:`\dot{p}` and :math:`\omega` are the linear and angular velocities, respectively. @@ -2545,69 +1986,51 @@ class Robot(object): return jacobian.dot(dq) # TODO: implement IK for several links (also by exploiting the null space) - def calculateInverseKinematics(self, linkId, targetPosition, targetOrientation=None, - lowerLimits=None, upperLimits=None, jointRanges=None, restPoses=None, - jointDamping=None, maxIter=1, threshold=1e-4): + def calculate_inverse_kinematics(self, link_id, position, orientation=None, lower_limits=None, upper_limits=None, + joint_ranges=None, rest_poses=None, joint_dampings=None, max_iters=1, + threshold=1e-4): """ Compute the FULL Inverse kinematics; it will return a position for all the actuated joints. Args: - linkId (int): link id - targetPosition (float[3]): target position - targetOrientation (float[4]): target orientation + link_id (int): end effector link index. + position (np.float[3]): target position of the end effector (its link coordinate, not center of mass + coordinate!). By default this is in Cartesian world space, unless you provide `q_curr` joint angles. + orientation (np.float[4]): target orientation in Cartesian world space, quaternion [x,y,w,z]. If not + specified, pure position IK will be used. + lower_limits (np.float[N], list of N floats): lower joint limits. Optional null-space IK. + upper_limits (np.float[N], list of N floats): upper joint limits. Optional null-space IK. + joint_ranges (np.float[N], list of N floats): range of value of each joint. + rest_poses (np.float[N], list of N floats): joint rest poses. Favor an IK solution closer to a given rest + pose. + joint_dampings (np.float[N], list of N floats): joint damping factors. Allow to tune the IK solution using + joint damping factors. + solver (int): p.IK_DLS (=0) or p.IK_SDLS (=1), Damped Least Squares or Selective Damped Least Squares, as + described in the paper by Samuel Buss "Selectively Damped Least Squares for Inverse Kinematics". + q_curr (np.float[N]): list of joint positions. By default PyBullet uses the joint positions of the body. + If provided, the target_position and targetOrientation is in local space! + max_iters (int): maximum number of iterations. Refine the IK solution until the distance between target + and actual end effector position is below this threshold, or the `max_iters` is reached. + threshold (float): residual threshold. Refine the IK solution until the distance between target and actual + end effector position is below this threshold, or the `max_iters` is reached. Returns: - float[M]: joint positions + np.float[M]: joint positions (for each actuated joint). """ - # build dictionary - d = {} + # calculate joint positions solving IK and return them + return self.sim.calculate_inverse_kinematics(self.id, link_id, position=position, orientation=orientation, + lower_limits=lower_limits, upper_limits=upper_limits, + joint_ranges=joint_ranges, rest_poses=rest_poses, + joint_dampings=joint_dampings, max_iters=max_iters, + threshold=threshold) - # orientation for IK - if targetOrientation is not None: - d[targetOrientation] = targetOrientation - - # null-space - if not (lowerLimits is None or upperLimits is None or jointRanges is None or restPoses is None): - d[lowerLimits], d[upperLimits], d[jointRanges], d[restPoses] = lowerLimits, upperLimits, jointRanges, \ - restPoses - - # for damped IK - if jointDamping is not None: - d[jointDamping] = jointDamping - - # perform IK - if maxIter < 2 or maxIter is None: - # calculate joint positions solving IK and return them - return np.array(self.sim.calculateInverseKinematics(self.id, linkId, targetPosition, **d)) - else: - # perform IK for a certain number of steps - closeEnough = False - iter, dist = 0, np.inf - - for iter in range(maxIter): - # calculate IK - q = self.sim.calculateInverseKinematics(self.id, linkId, targetPosition, **d) - - # set positions - self.setJointPositions(q, self.joints) - - # calculate position error in task/operation space - pos = self.getLinkPositions(linkId) - dist = np.linalg.norm(targetPosition - pos) - - # check if over - if dist < threshold: - break - - return q - - def hardPriorities(self, jacobians, taskVelocities, method='backtrack'): + def hard_priorities(self, jacobians, task_velocities, method='backtrack'): """ Return dq. Args: jacobians: - taskVelocities: + task_velocities: methods: 'successive', 'augmented', 'backtrack'. Returns: @@ -2619,7 +2042,7 @@ class Robot(object): # Dynamics # ############ - def calculateInverseDynamics(self, des_ddq, dq=None, q=None): + def calculate_inverse_dynamics(self, des_ddq, dq=None, q=None): r""" Starting from the specified joint positions :math:`q` and velocities :math:`\dot{q}`, it computes the joint torques :math:`\tau` required to reach the desired joint accelerations :math:`\ddot{q}_{des}`. That is, @@ -2650,12 +2073,12 @@ class Robot(object): of motion in task/operational space (instead of joint space), check the references [1-4]. Args: - q (float[M]): joint positions - dq (float[M]): joint velocities - des_ddq (float[M]): desired joint accelerations + q (np.float[M]): joint positions + dq (np.float[M]): joint velocities + des_ddq (np.float[M]): desired joint accelerations Returns: - float[M]: joint torques computed using the rigid-body equation of motion + np.float[M]: joint torques computed using the rigid-body equation of motion References: [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1 @@ -2666,25 +2089,14 @@ class Robot(object): """ # if the joint velocities and positions are not provided, read them if dq is None: - dq = self.getJointVelocities() + dq = self.get_joint_velocities() if q is None: - q = self.getJointPositions() - - # convert numpy arrays to lists - if isinstance(q, np.ndarray): - q = q.tolist() - if isinstance(dq, np.ndarray): - dq = dq.tolist() - if isinstance(des_ddq, np.ndarray): - des_ddq = des_ddq.tolist() + q = self.get_joint_positions() # return the joint torques to be applied for the desired joint accelerations - return np.array(self.sim.calculateInverseDynamics(self.id, q, dq, des_ddq)) + return self.sim.calculate_inverse_dynamics(self.id, q, dq, des_ddq) - # alias - calculateID = calculateInverseDynamics - - def calculateForwardDynamics(self, torques, dq=None, q=None): + def calculate_forward_dynamics(self, torques, dq=None, q=None): r""" Given the specified joint positions :math:`q` and velocities :math:`\dot{q}`, and joint torques :math:`\tau`, it computes the joint accelerations :math:`\ddot{q}`. That is, :math:`\ddot{q} = FD(model, q, \dot{q}, \tau)`. @@ -2715,12 +2127,12 @@ class Robot(object): of motion in task/operational space (instead of joint space), check the references [1-4]. Args: - q (float[M]): joint positions - dq (float[M]): joint velocities - torques (float[M]): desired joint torques + q (np.float[M]): joint positions + dq (np.float[M]): joint velocities + torques (np.float[M]): desired joint torques Returns: - float[M]: joint accelerations computed using the rigid-body equation of motion + np.float[M]: joint accelerations computed using the rigid-body equation of motion References: [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1 @@ -2731,21 +2143,18 @@ class Robot(object): """ # if the joint velocities and positions are not provided, read them if dq is None: - dq = self.getJointVelocities() + dq = self.get_joint_velocities() if q is None: - q = self.getJointPositions() + q = self.get_joint_positions() # compute and return joint accelerations torques = np.array(torques) - Hinv = np.linalg.inv(self.calculateMassMatrix(q)) - C = self.calculateInverseDynamics(np.zeros(len(q)), dq=dq, q=q) + Hinv = np.linalg.inv(self.get_mass_matrix(q)) + C = self.calculate_inverse_dynamics(np.zeros(len(q)), dq=dq, q=q) acc = Hinv.dot(torques - C) return acc - # alias - calculateFD = calculateForwardDynamics - - def calculateMassMatrix(self, q=None, qIdx=None): + def get_mass_matrix(self, q=None, q_idx=None): """ Return the mass/inertia matrix :math:`H(q)`. @@ -2755,32 +2164,29 @@ class Robot(object): Args: q (float[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will get the current joint positions (but note that this could lead to a decrease of performance). - qIdx (slice, None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N). + q_idx (slice, None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N). Returns: float[N,N], float[6+N,6+N], float[M,M]: inertia matrix """ if q is None: - q = self.getJointPositions() + q = self.get_joint_positions() else: - if len(q) != self.getNumberOfDoFs(): + if len(q) != self.num_dofs: raise ValueError("All the joint positions need to be given to this method. You can then slice the" "inertia matrix afterward.") # TODO: we need to get all the joints (even the fixed ones) --> need to test # make sure that we have all the joints even the fixed ones - qAug = np.zeros(self.getNumberOfJoints()) - qAug[self.joints] = q - qAug = qAug.tolist() # Note that pybullet doesn't accept numpy arrays here + q_aug = np.zeros(self.num_joints) + q_aug[self.joints] = q + q_aug = q_aug.tolist() # Note that pybullet doesn't accept numpy arrays here - if qIdx is None: - return np.array(self.sim.calculateMassMatrix(self.id, qAug)) - return np.array(self.sim.calculateMassMatrix(self.id, qAug))[qIdx, qIdx] + if q_idx is None: + return np.array(self.sim.calculate_mass_matrix(self.id, q_aug)) + return np.array(self.sim.calculate_mass_matrix(self.id, q_aug))[q_idx, q_idx] - # alias - getInertiaMatrix = calculateMassMatrix - - def getCartesianInertiaMatrix(self, H=None, Ja=None): + def get_cartesian_inertia_matrix(self, H=None, Ja=None): """ Return the cartesian inertia matrix. @@ -2795,7 +2201,7 @@ class Robot(object): H (float[N,N], None): Joint inertia matrix. If None, it will be computed here (the q's then need to be provided). Ja (float[6,N], None): Analytical Jacobian. If None, it will be computed here (the q's then need to be - provided and the linkId + provided and the link_id Returns: float[6,6]: Cartesian inertia matrix @@ -2803,7 +2209,7 @@ class Robot(object): Ja_inv = np.linalg.inv(Ja) return Ja_inv.T.dot(H).dot(Ja_inv) - def getKineticEnergy(self, q=None, dq=None, qIdx=None): + def get_kinetic_energy(self, q=None, dq=None, q_idx=None): """ Return the kinetic energy due to the movement of the specified joint(s). @@ -2814,20 +2220,20 @@ class Robot(object): get the current joint positions (but note that this could lead to a decrease of performance). dq (float[M], None): joint velocities of size M (with 0 < M <= N). If None, it will get the current joint velocities (but note that this could lead to a decrease of performance). - qIdx (slice, None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N), + q_idx (slice, None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N), and the joint velocities vector. Returns: float: kinetic energy """ if dq is None: - dq = self.getJointVelocities() - if qIdx is not None and len(dq) != len(qIdx): - dq = dq[qIdx] - H = self.calculateMassMatrix(q, qIdx) + dq = self.get_joint_velocities() + if q_idx is not None and len(dq) != len(q_idx): + dq = dq[q_idx] + H = self.get_mass_matrix(q, q_idx) return 1./2 * dq.dot(H.dot(dq)) - def getGravityPotentialEnergy(self, q=None, qIdx=None, g=np.array((0.,0.,-9.81))): + def get_gravity_potential_energy(self, q=None, q_idx=None, g=np.array((0., 0., -9.81))): """ Return the potential energy due to gravity. @@ -2839,42 +2245,43 @@ class Robot(object): Args: q (float[N], None): joint positions of size N, where N is the total number of DoFs. THIS IS CURRENTLY NOT USED, as we can get the link positions from the simulator (instead of using forward kinematics). - qIdx (int[M], None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N), + q_idx (int[M], None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N), and the joint velocities vector. + g (np.float[3]): gravity vector. Returns: float: potential energy due to gravity """ - linkIds = list(range(self.getNumberOfLinks())) - p = self.getLinkWorldPositions(linkId=linkIds, flatten=False) - m = self.getLinkMasses(linkId=linkIds) - if qIdx is not None: - p = p[self.joints[qIdx]] - m = m[self.joints[qIdx]] + link_ids = list(range(self.num_links)) + p = self.get_link_world_positions(link_ids=link_ids, flatten=False) + m = self.get_link_masses(link_ids=link_ids) + if q_idx is not None: + p = p[self.joints[q_idx]] + m = m[self.joints[q_idx]] return np.sum((p.T * m).T * g) - def getPotentialEnergy(self, q=None, dq=None, qIdx=None): + def get_potential_energy(self, q=None, dq=None, q_idx=None): """ Return the potential energy of the system. WARNING: Note that we currently assume rigid body systems (thus rigid links). With this assumption, the potential energy is only due to gravitational forces. So, for now this is just an alias to - `getGravityPotentialEnergy`. + `get_gravity_potential_energy`. Args: q (float[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will get the current joint positions (but note that this could lead to a decrease of performance). dq (float[M], None): joint velocities of size M (with 0 < M <= N). If None, it will get the current joint velocities (but note that this could lead to a decrease of performance). - qIdx (int[M], None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N), + q_idx (int[M], None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N), and the joint velocities vector. Returns: float: potential energy """ - return self.getGravityPotentialEnergy(q, qIdx) + return self.get_gravity_potential_energy(q, q_idx) - def getLagrangian(self, q=None, dq=None, qIdx=None): + def get_lagrangian(self, q=None, dq=None, q_idx=None): """ Return the Lagrangian evaluate at the given configuration. @@ -2887,17 +2294,17 @@ class Robot(object): get the current joint positions (but note that this could lead to a decrease of performance). dq (float[M], None): joint velocities of size M (with 0 < M <= N). If None, it will get the current joint velocities (but note that this could lead to a decrease of performance). - qIdx (int[M], None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N), + q_idx (int[M], None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N), and the joint velocities vector. Returns: float: value of the Lagrangian """ - T = self.getKineticEnergy(q=q, dq=dq, qIdx=qIdx) - V = self.getPotentialEnergy(q=q, qIdx=qIdx) + T = self.get_kinetic_energy(q=q, dq=dq, q_idx=q_idx) + V = self.get_potential_energy(q=q, q_idx=q_idx) return T - V - def applyExternalForce(self, force, linkId=-1, position=(0.,0.,0.), flag=1): + def apply_external_force(self, force, link_id=-1, position=(0., 0., 0.), frame=1): """ Apply an external force on a body, or a link of the body. Note that after each simulation step, the external forces are cleared to 0. @@ -2906,14 +2313,14 @@ class Robot(object): Args: force (float[3]): Cartesian forces to be applied on the body - linkId (int): link id to apply the force, if -1 it will apply the force on the base + link_id (int): link id to apply the force, if -1 it will apply the force on the base position (float[3]): position on the link where the force is applied. - frameFlag (int): allows to specify the coordinate system of force/position. sim.LINK_FRAME (=1) for local + frame (int): allows to specify the coordinate system of force/position. sim.LINK_FRAME (=1) for local link frame, and sim.WORLD_FRAME (=2) for world frame. By default, it is the link frame. """ - self.sim.applyExternalForce(self.id, linkId, force, position, flag) + self.sim.apply_external_force(self.id, link_id, force, position, frame) - def applyExternalTorque(self, torque, linkId=-1, flag=1): + def apply_external_torque(self, torque, link_id=-1, frame=1): """ Apply an external torque on a body, or a link of the body. Note that after each simulation step, the external torques are cleared to 0. @@ -2922,13 +2329,13 @@ class Robot(object): Args: torque (float[3]): Cartesian torques to be applied on the body - linkId (int): link id to apply the torque, if -1 it will apply the torque on the base - frameFlag (int): allows to specify the coordinate system of torque. sim.LINK_FRAME (=1) for local + link_id (int): link id to apply the torque, if -1 it will apply the torque on the base + frame (int): allows to specify the coordinate system of torque. sim.LINK_FRAME (=1) for local link frame, and sim.WORLD_FRAME (=2) for world frame. By default, it is the link frame. """ - self.sim.applyExternalTorque(self.id, linkId, force, flag) + self.sim.apply_external_torque(self.id, link_id, force, frame) - def getJointTorquesFromCartesianWrench(self, jacobian, wrench): + def get_joint_torques_from_cartesian_wrench(self, jacobian, wrench): """ Return the joint torques from the given Cartesian wrench (=force and torque) using the provided Jacobian. @@ -2942,7 +2349,7 @@ class Robot(object): """ return jacobian.T.dot(wrench) - def getCartesianWrenchFromJointTorques(self, jacobian, torque): + def get_cartesian_wrench_from_joint_torques(self, jacobian, torque): """ Return the Cartesian wrench (=force and torque) from the given joint torques using the provided Jacobian. @@ -2957,7 +2364,7 @@ class Robot(object): J = jacobian return J.dot(np.linalg.inv(J.T.dot(J))).dot(torque) - def enableCoriolisAndGravityCompensation(self, enable=True): + def enable_coriolis_and_gravity_compensation(self, enable=True): """ Enable the gravity and Coriolis compensation when applying torques. This will automatically compute these terms and add them automatically to the given torques when using torque control. @@ -2965,9 +2372,9 @@ class Robot(object): Args: enable (bool): If True, enable the gravity and Coriolis compensation when applying torques. """ - self.coriolisAndGravityCompensation = enable + self.coriolis_and_gravity_compensation = enable - def getCoriolisAndGravityCompensationTorques(self, q=None, dq=None, qIdx=None): + def get_coriolis_and_gravity_compensation_torques(self, q=None, dq=None, qIdx=None): """ Return the torques that need to be applied to the robot joints such that it compensates for gravity and Coriolis effects. @@ -2995,9 +2402,9 @@ class Robot(object): float[M]: joint torques to be applied [Nm] """ if q is None: - q = self.getJointPositions() + q = self.get_joint_positions() if dq is None: - dq = self.getJointVelocities() + dq = self.get_joint_velocities() ddq = np.zeros(len(self.joints)).tolist() @@ -3007,16 +2414,16 @@ class Robot(object): dq = dq.tolist() if qIdx is None: - return np.array(self.sim.calculateInverseDynamics(self.id, q, dq, ddq)) - return np.array(self.sim.calculateInverseDynamics(self.id, q, dq, ddq))[qIdx] + return self.sim.calculate_inverse_dynamics(self.id, q, dq, ddq) + return self.sim.calculate_inverse_dynamics(self.id, q, dq, ddq)[qIdx] - def getGravityCompensationTorques(self, q=None, qIdx=None): + def get_gravity_compensation_torques(self, q=None, qIdx=None): if q is None: - q = self.getJointPositions() + q = self.get_joint_positions() dq = np.zeros(len(q)) - return self.getCoriolisAndGravityCompensationTorques(q, dq, qIdx) + return self.get_coriolis_and_gravity_compensation_torques(q, dq, qIdx) - def applyCoriolisAndGravityCompensation(self, q=None, dq=None, qIdx=None, external_torques=0.): + def apply_coriolis_and_gravity_compensation(self, q=None, dq=None, qIdx=None, external_torques=0.): """ Apply Coriolis and Gravity Compensation; set the torques using torque control. @@ -3033,11 +2440,11 @@ class Robot(object): qIdx (int[M], None): slice the torques at the given q indices (0 < M <= N). """ jointId = self.joints if qIdx is None else self.joints[qIdx] - torques = self.getCoriolisAndGravityCompensationTorques(q,dq,qIdx) - self.setJointTorques(jointId, torques + external_torques) + torques = self.get_coriolis_and_gravity_compensation_torques(q, dq, qIdx) + self.set_joint_torques(jointId, torques + external_torques) # TODO: finish to implement the method + think about multiple links + think about dimensions - def getActiveCompliantTorques(self, q=None, dq=None, qIdx=None, jacobian=None, linkVelocity=None, linkId=None, kd=60): + def get_active_compliant_torques(self, q=None, dq=None, qIdx=None, jacobian=None, linkVelocity=None, link_ids=None, kd=60): """ Return the torques that need to be applied to enable active compliance. This is done by enabling Coriolis and gravity compensation along with a damping force projected from the Cartesian space to the joint space. @@ -3060,22 +2467,22 @@ class Robot(object): float[M]: joint torques to be applied [Nm] """ if q is None: - q = self.getJointPositions() + q = self.get_joint_positions() if dq is None: - dq = self.getJointVelocities() + dq = self.get_joint_velocities() if jacobian is None: - jacobian = self.calculateJacobian(linkId, q) + jacobian = self.get_jacobian(link_id, q) if linkVelocity is None: - linkVelocity = self.getLinkWorldVelocities(linkId) + linkVelocity = self.get_link_world_velocities(link_id) if isinstance(kd, int): kd = kd * np.identity(6) - torques = self.getCoriolisAndGravityCompensationTorques(q, dq, qIdx) + torques = self.get_coriolis_and_gravity_compensation_torques(q, dq, qIdx) torques += jacobian.T.dot(-kd * linkVelocity) return torques # TODO: finish to implement the method - def applyActiveCompliance(self, q=None, dq=None, qIdx=None, external_torques=0.): + def apply_active_compliance(self, q=None, dq=None, qIdx=None, external_torques=0.): """ Apply active compliance; this is done by enabling Coriolis and gravity compensation along with a damping force projected from the Cartesian space to the joint space. @@ -3089,22 +2496,22 @@ class Robot(object): qIdx (int[M], None): slice the torques at the given q indices (0 < M <= N). """ jointId = self.joints if qIdx is None else self.joints[qIdx] - torques = self.getActiveCompliantTorques(q, dq, qIdx) - self.setJointTorques(jointId, torques + external_torques) + torques = self.get_active_compliant_torques(q, dq, qIdx) + self.set_joint_torques(jointId, torques + external_torques) - def getImpedanceTorques(self, x=0, dx=0, ddx=0): + def get_impedance_torques(self, x=0, dx=0, ddx=0): """ .. math:: F_{a} = H_m (\ddot{x} - \ddot{x}_d) + D_m (\dot{x} - \dot{x}_d) + K_m (x - x_d) """ - raise NotImplementedError + pass - def applyTaskImpedanceControl(self): + def apply_task_impedance_control(self): """ .. math:: F_{a} = H_m (\ddot{x} - \ddot{x}_d) + D_m (\dot{x} - \dot{x}_d) + K_m (x - x_d) """ - raise NotImplementedError + pass - def getAttractorTorques(self): + def get_attractor_torques(self): """ The torques to be applied are given by: @@ -3113,13 +2520,13 @@ class Robot(object): where :math:`F = K(x_d - x) - D v` with :math:`x` and :math:`v` are the Cartesian position and velocities, and :math:`K` and :math:`D` are the stiffness and damping factor, respectively. """ - raise NotImplementedError + pass ###################### # Symbolic Equations # ###################### - def getSymbolicEquationsOfMotion(self): + def get_symbolic_equations_of_motion(self): """ This returns the symbolic equation of motions of the robot (using the URDF). Internally, this used the `sympy.mechanics` module. @@ -3133,10 +2540,7 @@ class Robot(object): """ pass - # alias - getSymbolicEOM = getSymbolicEquationsOfMotion - - def linearizeEquationOfMotion(self, point=None): + def linearize_equations_of_motion(self, point=None): """ Linearize the equation of motions around the given point. That is, instead of having :math:`\dot{x} = f(x,u)` where :math:`f` is in general a non-linear function, linearize it around a certain point. @@ -3162,14 +2566,12 @@ class Robot(object): """ pass - # alias - linearizeEOM = linearizeEquationOfMotion - ########### # Sensors # ########### - def getNumberOfSensors(self): + @property + def num_sensors(self): """ Return the total number of sensors. @@ -3178,7 +2580,7 @@ class Robot(object): """ return len(self.sensors) - def enableJointForceTorqueSensor(self, jointId=None, enable=True): + def enable_joint_force_torque_sensor(self, joint_ids=None, enable=True): """ Enable/disable the force/torque sensors of the specified joint(s). @@ -3186,19 +2588,15 @@ class Robot(object): useful for debug among other things. Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, enable/disable the F/T sensors + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, enable/disable the F/T sensors on all the actuated joints. enable (bool): If True, it will enable the F/T sensors, otherwise it will disable them. """ - if isinstance(jointId, int): - self.sim.enableJointForceTorqueSensor(self.id, jointId, enableSensor=enable) - else: - if jointId is None: - jointId = self.joints - for joint in jointId: - self.sim.enableJointForceTorqueSensor(self.id, joint, enableSensor=enable) + if joint_ids is None: + joint_ids = self.joints + self.sim.enable_joint_force_torque_sensor(self.id, joint_ids, enable=enable) - def disableJointForceTorqueSensor(self, jointId=None): + def disable_joint_force_torque_sensor(self, joint_ids=None): """ Disable the force/torque sensors of the specified joint(s). @@ -3206,18 +2604,14 @@ class Robot(object): useful for debug among other things. Args: - jointId (int, int[N], None): joint id, or list of joint ids. If None, disable the F/T sensors on all the + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, disable the F/T sensors on all the actuated joints. """ - if isinstance(jointId, int): - self.sim.enableJointForceTorqueSensor(self.id, jointId, enableSensor=0) - else: - if jointId is None: - jointId = self.joints - for joint in jointId: - self.sim.enableJointForceTorqueSensor(self.id, joint, enableSensor=0) + if joint_ids is None: + joint_ids = self.joints + self.sim.enable_joint_force_torque_sensor(self.id, joint_ids, enable=False) - def getSensors(self, idx=None): + def get_sensors(self, idx=None): """ Return the specified sensor. @@ -3231,32 +2625,33 @@ class Robot(object): return self.sensors return self.sensors[idx] - def getIMU(self, idx=0): - raise NotImplementedError + def get_imu(self, idx=0): + pass - def getForceTorqueSensor(self, idx=0): - raise NotImplementedError + def get_force_torque_sensor(self, idx=0): + pass - def hasCamera(self): + def has_camera(self): return False - def getCamera(self, idx=0): - raise NotImplementedError + def get_camera(self, idx=0): + pass - def getCameraImage(self, idx=0): - raise NotImplementedError + def get_camera_image(self, idx=0): + pass - def getMainCamera(self): - raise NotImplementedError + def get_main_camera(self): + pass - def getMainCameraImage(self): - raise NotImplementedError + def get_main_camera_image(self): + pass ############# # Actuators # ############# - def getNumberOfActuators(self): + @property + def num_actuators(self): """ Return the total number of actuators. @@ -3265,7 +2660,7 @@ class Robot(object): """ return len(self.actuators) - def getActuators(self, idx=None): + def get_actuators(self, idx=None): """ Return the specified actuator. @@ -3283,7 +2678,7 @@ class Robot(object): # Contacts/Collisions # ####################### - def getContacts(self): + def get_contacts(self): """ Return all the contacts made by the robot. @@ -3303,246 +2698,262 @@ class Robot(object): float: contact distance (positive for separation and negative for penetration) float: normal force applied during the last simulation step """ - return self.sim.getContactPoints(bodyA=self.id) + return self.sim.get_contact_points(body1=self.id) ######### # Debug # ######### - def printJointInfo(self, jointId): + def _get_joint_type_str(self, idx): + """ + Return the joint type as a string based on the flag. + + Args: + idx (int): flag for the type of joint + + Returns: + str: name of the joint type + """ + return ['revolute', 'prismatic', 'spherical', 'planar', 'fixed', 'point2point', 'gear'][idx] + + def print_joint_info(self, joint_id): """ Print information about the given joint. Args: - jointId (int): unique joint id. + joint_id (int): unique joint id. """ - jnt = self.sim.getJointInfo(self.id, jointId) - print('joint index: {}'.format(jnt[0])) - print('joint name: {}'.format(jnt[1])) - print('joint type: {}'.format(self.getJointTypeStr(jnt[2]))) - print('q index: {}'.format(jnt[3])) - print('qd index: {}'.format(jnt[4])) - print('joint damping: {}'.format(jnt[6])) - print('joint friction: {}'.format(jnt[7])) - print('joint lower limit: {}'.format(jnt[8])) - print('joint upper limit: {}'.format(jnt[9])) - print('joint max force: {}'.format(jnt[10])) - print('joint max velocity: {}'.format(jnt[11])) - print('associated link name: {}'.format(jnt[12])) - print('joint axis: {}'.format(jnt[13])) - print('position wrt parent frame: {}'.format(jnt[14])) - print('orientation wrt parent frame: {}'.format(jnt[15])) - print('parent link index: {}'.format(jnt[16])) + joint = self.sim.get_joint_info(self.id, joint_id) + print('joint index: {}'.format(joint[0])) + print('joint name: {}'.format(joint[1])) + print('joint type: {}'.format(self._get_joint_type_str(joint[2]))) + print('q index: {}'.format(joint[3])) + print('qd index: {}'.format(joint[4])) + print('joint damping: {}'.format(joint[6])) + print('joint friction: {}'.format(joint[7])) + print('joint lower limit: {}'.format(joint[8])) + print('joint upper limit: {}'.format(joint[9])) + print('joint max force: {}'.format(joint[10])) + print('joint max velocity: {}'.format(joint[11])) + print('associated link name: {}'.format(joint[12])) + print('joint axis: {}'.format(joint[13])) + print('position wrt parent frame: {}'.format(joint[14])) + print('orientation wrt parent frame: {}'.format(joint[15])) + print('parent link index: {}'.format(joint[16])) - def printLinkInfo(self, linkId): + def print_link_info(self, link_id): """ Print information about the given link. The information printed include the link frame position and orientation, its center of mass position and orientation, its dimensions, its mass, its local inertia diagonal, etc. Args: - linkId (int): unique link id + link_id (int): unique link id """ - pass + state = self.sim.get_link_state(self.id, link_id) + print('link name: {}'.format(self.sim.get_link_names(self.id, link_id))) + print('link world position: {}'.format(state[0])) + print('link world orientation: {}'.format(state[1])) + print('link inertial frame position: {}'.format(state[2])) + print('link inertial frame orientation: {}'.format(state[3])) + print('world link frame position: {}'.format(state[4])) + print('world link frame orientation: {}'.format(state[5])) + print('world link linear velocity: {}'.format(state[6])) + print('world link angular velocity: {}'.format(state[7])) - def printRobotInfo(self): + def print_info(self): """ Print general information about the robot. """ print("\nRobot: {}".format(self)) - print("Number of DoFs: {}".format(self.getNumberOfDoFs())) - print("Joint ids: {}".format(list(range(self.getNumberOfJoints())))) + print("Number of DoFs: {}".format(self.num_dofs)) + print("Joint ids: {}".format(list(range(self.num_joints)))) print("Actuated joint ids: {}".format(self.joints)) - print("Link names (associated with actuated joints): {}".format(self.getLinkNames(self.joints))) - print("End-effector names: {}".format(self.getEndEffectorNames())) - print("Floating base? {}".format(self.hasFloatingBase())) - print("Total mass = {} kg".format(self.getTotalMass())) + print("Link names (associated with actuated joints): {}".format(self.get_link_names(self.joints))) + print("End-effector names: {}".format(self.get_link_names(self.end_effectors))) + print("Floating base? {}".format(self.has_floating_base())) + print("Total mass = {} kg".format(self.mass)) - def addJointSlider(self, jointId=None): + def add_joint_slider(self, joint_ids=None): """ Add a slider for the given joint id. Args: - jointId (int, str, list of str/int, None): if int, the id is between {0, N} where N=number of non-fixed + joint_ids (int, str, list of str/int, None): if int, the id is between {0, N} where N=number of non-fixed joint. If str, the name of the joint. If list/tuple, it contains the id or name of the joints. If None, add a slider for each non-fixed joint. """ # show debug visualizer - self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_GUI, 1) + self.sim.configure_debug_visualizer(self.sim.COV_ENABLE_GUI, 1) def getIndex(jnt): if isinstance(jnt, int): # joint id return jnt elif isinstance(jnt, str): # joint name - return self.getJointIds(jnt) + return self.get_joint_ids(jnt) else: raise TypeError('Expecting a str or int for the joint: {}'.format(jnt)) # get the joint indices - if jointId is None: - jointId = self.joints + if joint_ids is None: + joint_ids = self.joints else: - if isinstance(jointId, int): # joint id - jointId = [jointId] - elif isinstance(jointId, str): # joint name - jointId = [self.getJointIds(jointId)] - elif isinstance(jointId, collections.Iterable): - jointId = [getIndex(jnt) for jnt in jointId] + if isinstance(joint_ids, int): # joint id + joint_ids = [joint_ids] + elif isinstance(joint_ids, str): # joint name + joint_ids = [self.get_joint_ids(joint_ids)] + elif isinstance(joint_ids, collections.Iterable): + joint_ids = [getIndex(jnt) for jnt in joint_ids] else: raise TypeError("jointId has to be a None, int, str, or a list/tuple of int/str.") - # get informations about the joints - names = self.getJointNames(jointId) - limits = self.getJointLimits(jointId) - positions = self.getJointPositions(jointId) + # get information about the joints + names = self.get_joint_names(joint_ids) + limits = self.get_joint_limits(joint_ids) + positions = self.get_joint_positions(joint_ids) lower_limits, upper_limits = limits[:, 0], limits[:, 1] - # # apply lower and upper limit + # apply lower and upper limit lower_limits[lower_limits < -2 * np.pi] = -2. * np.pi upper_limits[upper_limits > 2 * np.pi] = 2. * np.pi - # add sliders in pybullet - for i in range(len(jointId)): - slider = self.sim.addUserDebugParameter(names[i], lower_limits[i], upper_limits[i], positions[i]) - self.joint_sliders[jointId[i]] = slider + # add sliders in PyBullet + for i in range(len(joint_ids)): + slider = self.sim.add_user_debug_parameter(names[i], lower_limits[i], upper_limits[i], positions[i]) + self.joint_sliders[joint_ids[i]] = slider - def updateJointSlider(self): + def update_joint_slider(self): """ Read the specified joint slider value, and set the robot's corresponding joint to this one using position control. """ # for each slider - for jointId, slider in self.joint_sliders.items(): + for joint_id, slider in self.joint_sliders.items(): # read joint value from slider - pos = self.sim.readUserDebugParameter(slider) + pos = self.sim.read_user_debug_parameter(slider) # set joint position to the read value - self.sim.setJointMotorControl2(self.id, jointId, self.sim.POSITION_CONTROL, targetPosition=pos) + self.sim.set_joint_motor_control(self.id, joint_id, self.sim.POSITION_CONTROL, positions=pos) # alias - readJointSlider = updateJointSlider + read_joint_slider = update_joint_slider - def removeJointSlider(self, jointId=None): + def remove_joint_slider(self, joint_ids=None): """ Remove the specified joint slider(s). Args: - jointId (int, str, list of str/int, None): if int, the id is between {0, N} where N=number of non-fixed + joint_ids (int, str, list of str/int, None): if int, the id is between {0, N} where N=number of non-fixed joint. If str, the name of the joint. If list/tuple, it contains the id or name of the joints. If None, add a slider for each non-fixed joint. """ - def getIndex(jnt): - if isinstance(jnt, int): # joint id - return jnt - elif isinstance(jnt, str): # joint name - return self.getJointIds(jnt) + def get_index(joint): + if isinstance(joint, int): # joint id + return joint + elif isinstance(joint, str): # joint name + return self.get_joint_ids(joint) else: - raise TypeError('Expecting a str or int for the joint: {}'.format(jnt)) + raise TypeError('Expecting a str or int for the joint: {}'.format(joint)) # get the joint indices - if jointId is None: - jointId = self.joints + if joint_ids is None: + joint_ids = self.joints else: - if isinstance(jointId, int): # joint id - jointId = [jointId] - elif isinstance(jointId, str): # joint name - jointId = [self.getJointIds(jointId)] - elif isinstance(jointId, collections.Iterable): - jointId = [getIndex(jnt) for jnt in jointId] + if isinstance(joint_ids, int): # joint id + joint_ids = [joint_ids] + elif isinstance(joint_ids, str): # joint name + joint_ids = [self.get_joint_ids(joint_ids)] + elif isinstance(joint_ids, collections.Iterable): + joint_ids = [get_index(joint) for joint in joint_ids] else: raise TypeError("jointId has to be a None, int, str, or a list/tuple of int/str.") # remove sliders in pybullet - for joint in jointId: + for joint in joint_ids: if joint in self.joint_sliders: - self.sim.removeUserDebugItem(self.joint_sliders[joint]) + self.sim.remove_user_debug_item(self.joint_sliders[joint]) self.joint_sliders.pop(joint) # if no sliders anymore, remove the debug visualizer - self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_GUI, 0) + self.sim.configure_debug_visualizer(self.sim.COV_ENABLE_GUI, 0) #################### # online plotting # # WARNING: ALL THE FOLLOWING METHODS NEED A SIMULATOR IN WHICH TO RUN # #################### - def plotJointPositions(self, jointId=None): - raise NotImplementedError + def plot_joint_positions(self, joint_ids=None): + pass - def plotJointVelocities(self, jointId=None): - raise NotImplementedError + def plot_joint_velocities(self, joint_ids=None): + pass - def plotJointAccelerations(self, jointId=None): - raise NotImplementedError + def plot_joint_accelerations(self, joint_ids=None): + pass - def plotCoMPosition(self): - raise NotImplementedError + def plot_com_position(self): + pass - def plotCoMVelocity(self): - raise NotImplementedError + def plot_com_velocity(self): + pass - def plotCoMAcceleration(self): - raise NotImplementedError + def plot_com_acceleration(self): + pass - def plotCartesianPositions(self, linkId=None): - raise NotImplementedError + def plot_cartesian_positions(self, link_ids=None): + pass - def plotCartesianVelocities(self, linkId=None): - raise NotImplementedError + def plot_cartesian_velocities(self, link_ids=None): + pass - def plotCartesianAccelerations(self, linkId=None): - raise NotImplementedError + def plot_cartesian_accelerations(self, link_ids=None): + pass ######## # draw # # WARNING: ALL THE FOLLOWING METHODS NEED A SIMULATOR IN WHICH TO RUN # ######## - def _drawSphere(self, position, radius=0.1, color=(1,1,1,1)): - visual = self.sim.createVisualShape(self.sim.GEOM_SPHERE, radius=radius, rgbaColor=color) - body = self.sim.createMultiBody(baseMass=0, baseVisualShapeIndex=visual, basePosition=position) + def _draw_sphere(self, position, radius=0.1, color=(1, 1, 1, 1)): + visual = self.sim.create_visual_shape(self.sim.GEOM_SPHERE, radius=radius, rgba_color=color) + body = self.sim.create_body(mass=0, visual_shape_id=visual, position=position) return body - def _drawCylinder(self, position, orientation, radius=1, height=1, color=(1,1,1,1)): - visual = self.sim.createVisualShape(self.sim.GEOM_CYLINDER, radius=radius, length=height, rgbaColor=color) - orientation = self.angular_converter.convertFrom(orientation) # convert to list - body = self.sim.createMultiBody(baseMass=0., baseVisualShapeIndex=visual, basePosition=position, - baseOrientation=orientation) + def _draw_cylinder(self, position, orientation, radius=1, height=1, color=(1, 1, 1, 1)): + visual = self.sim.create_visual_shape(self.sim.GEOM_CYLINDER, radius=radius, length=height, rgba_color=color) + body = self.sim.create_body(mass=0., visual_shape_id=visual, position=position, orientation=orientation) return body - def _drawFrame(self, position, orientation, radius, length): - # quat = self.angular_converter.convertFrom(orientation) - # R = np.array(self.sim.getMatrixFromQuaternion(quat)).reshape(3,3) - R = quaternion.as_rotation_matrix(orientation) - # H = np.vstack((np.hstack((R, position.reshape(-1,1))), np.array([[0,0,0,1]]))) - x = R.dot(np.array([length/2., 0, 0])) + position # H.dot(np.array([length/2., 0, 0, 1]))[:3] - y = R.dot(np.array([0, length/2., 0])) + position # H.dot(np.array([0, length/2., 0, 1]))[:3] - z = R.dot(np.array([0, 0, length/2.])) + position # H.dot(np.array([0, 0, length/2., 1]))[:3] + def _draw_frame(self, position, orientation, radius, length): + R = get_matrix_from_quaternion(orientation) - qx = quaternion.quaternion(0.707, 0.707, 0, 0) # 90deg around x - qy = quaternion.quaternion(0.707, 0, 0.707, 0) # 90deg around y + x = R.dot(np.array([length/2., 0, 0])) + position + y = R.dot(np.array([0, length/2., 0])) + position + z = R.dot(np.array([0, 0, length/2.])) + position + + qx = np.array([0.707, 0, 0, 0.707]) # 90deg around x + qy = np.array([0, 0.707, 0, 0.707]) # 90deg around y # draw x, y, z cylinders - # self._drawCylinder(x, qy*orientation, radius, length, color=(1,0,0,1)) - self._drawCylinder(x, orientation*qy, radius, length, color=(1, 0, 0, 1)) - self._drawCylinder(y, orientation*qx, radius, length, color=(0, 1, 0, 1)) - self._drawCylinder(z, orientation, radius, length, color=(0, 0, 1, 1)) + self._draw_cylinder(x, get_quaternion_product(orientation, qy), radius, length, color=(1, 0, 0, 1)) + self._draw_cylinder(y, get_quaternion_product(orientation, qx), radius, length, color=(0, 1, 0, 1)) + self._draw_cylinder(z, orientation, radius, length, color=(0, 0, 1, 1)) - def _drawDebugBox(self, bbMin, bbMax): - (x0, y0, z0), (xf, yf, zf) = bbMin, bbMax - self.sim.addUserDebugLine((x0, y0, z0), (x0, yf, z0), (1, 1, 1)) - self.sim.addUserDebugLine((x0, yf, z0), (x0, yf, zf), (1, 1, 1)) - self.sim.addUserDebugLine((x0, yf, zf), (x0, y0, zf), (1, 1, 1)) - self.sim.addUserDebugLine((x0, y0, zf), (x0, y0, z0), (1, 1, 1)) + def _draw_debug_box(self, aabb_min, aabb_max): + (x0, y0, z0), (xf, yf, zf) = aabb_min, aabb_max + self.sim.add_user_debug_line((x0, y0, z0), (x0, yf, z0), (1, 1, 1)) + self.sim.add_user_debug_line((x0, yf, z0), (x0, yf, zf), (1, 1, 1)) + self.sim.add_user_debug_line((x0, yf, zf), (x0, y0, zf), (1, 1, 1)) + self.sim.add_user_debug_line((x0, y0, zf), (x0, y0, z0), (1, 1, 1)) - self.sim.addUserDebugLine((xf, y0, z0), (xf, yf, z0), (1, 1, 1)) - self.sim.addUserDebugLine((xf, yf, z0), (xf, yf, zf), (1, 1, 1)) - self.sim.addUserDebugLine((xf, yf, zf), (xf, y0, zf), (1, 1, 1)) - self.sim.addUserDebugLine((xf, y0, zf), (xf, y0, z0), (1, 1, 1)) + self.sim.add_user_debug_line((xf, y0, z0), (xf, yf, z0), (1, 1, 1)) + self.sim.add_user_debug_line((xf, yf, z0), (xf, yf, zf), (1, 1, 1)) + self.sim.add_user_debug_line((xf, yf, zf), (xf, y0, zf), (1, 1, 1)) + self.sim.add_user_debug_line((xf, y0, zf), (xf, y0, z0), (1, 1, 1)) - self.sim.addUserDebugLine((x0, y0, z0), (xf, y0, z0), (1, 1, 1)) - self.sim.addUserDebugLine((x0, yf, z0), (xf, yf, z0), (1, 1, 1)) - self.sim.addUserDebugLine((x0, y0, zf), (xf, y0, zf), (1, 1, 1)) - self.sim.addUserDebugLine((x0, yf, zf), (xf, yf, zf), (1, 1, 1)) + self.sim.add_user_debug_line((x0, y0, z0), (xf, y0, z0), (1, 1, 1)) + self.sim.add_user_debug_line((x0, yf, z0), (xf, yf, z0), (1, 1, 1)) + self.sim.add_user_debug_line((x0, y0, zf), (xf, y0, zf), (1, 1, 1)) + self.sim.add_user_debug_line((x0, yf, zf), (xf, yf, zf), (1, 1, 1)) - def changeTransparency(self, alpha=0.5): + def change_transparency(self, alpha=0.5): """ Change the transparency of a robot. @@ -3551,19 +2962,19 @@ class Robot(object): Args: alpha (float): alpha channel. 1 is opaque, and 0 is completely transparent. """ - for shapeId in self.visualShapes: - rgba = self.visualShapes[shapeId]['color'] + for shapeId in self.visual_shapes: + rgba = self.visual_shapes[shapeId]['color'] rgba[-1] = alpha - self.sim.changeVisualShape(self.id, shapeId, rgbaColor=rgba) + self.sim.change_visual_shape(self.id, shapeId, rgba_color=rgba) # print("Link {} - color: {}".format(link, rgba)) - def updateVisual(self): + def update_visual(self): """ Update all visuals. """ pass - def computeAndDrawCoMPosition(self, radius=0.05, color=(1,0,0,1)): + def compute_and_draw_com_position(self, radius=0.05, color=(1, 0, 0, 1)): """ Compute the CoM and draw it as a sphere in the simulator. @@ -3574,38 +2985,36 @@ class Robot(object): Returns: float[3]: center of mass """ - self.getCoMPosition() - self.drawCoMPosition(radius=radius, color=color) + self.get_center_of_mass_position() + self.draw_com_position(radius=radius, color=color) return self.com - def drawCoMPosition(self, radius=0.05, color=(1, 0, 0, 1)): + def draw_com_position(self, radius=0.05, color=(1, 0, 0, 1)): """ Draw the CoM in the simulator. - WARNING: `getCoMPosition()` must be called before calling this method. Otherwise, check the other method - `computeAndDrawCoMPosition()`. + WARNING: `get_center_of_mass_position()` must be called before calling this method. Otherwise, check the other method + `compute_and_draw_com_position()`. Args: radius (float): radius of the sphere representing the CoM of the robot color (float[4]): rgba color of the sphere. By default it is red. """ - if self.comVisual is None: # create visual shape if not already created - comVisualShape = self.sim.createVisualShape(self.sim.GEOM_SPHERE, radius=radius, rgbaColor=color) - self.comVisual = self.sim.createMultiBody(baseMass=0, - baseVisualShapeIndex=comVisualShape, - basePosition=self.com) + if self.com_visual is None: # create visual shape if not already created + com_visual_shape = self.sim.create_visual_shape(self.sim.GEOM_SPHERE, radius=radius, rgba_color=color) + self.com_visual = self.sim.create_body(mass=0, visual_shape_id=com_visual_shape, position=self.com) else: # set CoM position - self.sim.resetBasePositionAndOrientation(self.comVisual, self.com, [0, 0, 0, 1]) + self.sim.reset_base_pose(self.com_visual, self.com, [0, 0, 0, 1]) - def removeCoM(self): + def remove_com(self): """ Remove the CoM from the simulator. """ - if self.comVisual is not None: - self.sim.removeBody(self.comVisual) - self.comVisual = None + if self.com_visual is not None: + self.sim.remove_body(self.com_visual) + self.com_visual = None - def getProjectedCoMPosition(self, max_depth=5): + def get_projected_com_position(self, max_depth=5): """ Get the projected center of mass position. @@ -3617,14 +3026,14 @@ class Robot(object): Returns: float[3], None: position of the projected CoM, or None if it couldn't project the CoM """ - com = self.getCoMPosition() - object_id, _, _, hit_position, _ = self.sim.rayTest(com, com - np.array([0.,0.,max_depth]))[0] - if object_id >= 0: # if there is a collision - return hit_position # = projected com + com = self.get_center_of_mass_position() + object_id, _, _, hit_position, _ = self.sim.ray_test(com, com - np.array([0., 0., max_depth]))[0] + if object_id >= 0: # if there is a collision + return hit_position # = projected com else: return None - def computeAndDrawProjectedCoMPosition(self, radius=0.05, color=(0,0,1,1)): + def compute_and_draw_projected_com_position(self, radius=0.05, color=(0, 0, 1, 1)): """ Compute and draw the projected center of mass. @@ -3635,28 +3044,27 @@ class Robot(object): Returns: float[3], None: position of the projected CoM, or None if it couldn't project the CoM """ - projected_com = self.getProjectedCoMPosition() + projected_com = self.get_projected_com_position() if projected_com is not None: # if visual shape not already created, create one - if self.projectedCoMVisual is None: - visualShape = self.sim.createVisualShape(self.sim.GEOM_SPHERE, radius=radius, rgbaColor=color) - self.projectedCoMVisual = self.sim.createMultiBody(baseMass=0, - baseVisualShapeIndex=visualShape, - basePosition=projected_com) + if self.projected_com_visual is None: + visual_shape = self.sim.create_visual_shape(self.sim.GEOM_SPHERE, radius=radius, rgba_color=color) + self.projected_com_visual = self.sim.create_body(mass=0, visual_shape_id=visual_shape, + position=projected_com) # otherwise update projected CoM position else: - self.sim.resetBasePositionAndOrientation(self.projectedCoMVisual, projected_com, [0, 0, 0, 1]) + self.sim.reset_base_pose(self.projected_com_visual, projected_com, [0, 0, 0, 1]) return projected_com - def removeProjectedCoM(self): + def remove_projected_com(self): """ Remove the projected CoM from the simulator. """ - if self.projectedCoMVisual is not None: - self.sim.removeBody(self.projectedCoMVisual) - self.projectedCoMVisual = None + if self.projected_com_visual is not None: + self.sim.remove_body(self.projected_com_visual) + self.projected_com_visual = None # def drawProjectedCoM(self, radius=0.05, color=(1,0,0,1)): # """ @@ -3664,82 +3072,82 @@ class Robot(object): # """ # pass - def drawLinkCoMs(self, linkId=None, scaling=1.): + def draw_link_coms(self, link_ids=None, scaling=1.): """ Draw the CoM of the given link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get links associated to + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get links associated to actuated joints. scaling (float): scaling factor """ - if linkId is None: - linkId = self.joints - elif isinstance(linkId, int): - linkId = [linkId] + if link_ids is None: + link_ids = self.joints + elif isinstance(link_ids, int): + link_ids = [link_ids] - for link in linkId: - if link in self.visualShapes: - pos = self.getLinkWorldPositions(link) - dim = self.visualShapes[link]['dimensions'] + for link in link_ids: + if link in self.visual_shapes: + pos = self.get_link_world_positions(link) + dim = self.visual_shapes[link]['dimensions'] # radius = min(dim) * scaling * 0.2 radius = 0.01 - self._drawSphere(pos, radius, color=(0,0,0,1)) + self._draw_sphere(pos, radius, color=(0, 0, 0, 1)) - def drawLinkFrames(self, linkId=None, scaling=1.): + def draw_link_frames(self, link_ids=None, scaling=1.): """ Draw frames of the given link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get links associated to + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get links associated to actuated joints. scaling (float): scaling factor """ - if linkId is None: - linkId = [-1] + self.joints - elif isinstance(linkId, int): - linkId = [linkId] + if link_ids is None: + link_ids = [-1] + self.joints + elif isinstance(link_ids, int): + link_ids = [link_ids] - for link in linkId: - if link in self.visualShapes: + for link in link_ids: + if link in self.visual_shapes: if link == -1: - pos, orientation = self.getBasePositionAndOrientation() + pos, orientation = self.get_base_pose() else: - pos = self.getLinkFrameWorldPositions(link) - orientation = self.getLinkFrameWorldOrientations(link) - dim = self.visualShapes[link]['dimensions'] + pos = self.get_link_frame_world_positions(link) + orientation = self.get_link_frame_world_orientations(link) + dim = self.visual_shapes[link]['dimensions'] # radius = min(dim) * scaling * 0.2 radius = 0.005 * scaling - # self._drawSphere(pos, radius, color=(0,0,0,1)) + # self._draw_sphere(pos, radius, color=(0,0,0,1)) # length = 4*radius length = 0.05 * scaling - self._drawFrame(pos, orientation, radius, length) + self._draw_frame(pos, orientation, radius, length) - def drawJointFrames(self, jointId=None): + def draw_joint_frames(self, joint_ids=None): """ Draw (actuated) joint frame """ pass - def drawBoundingBoxes(self, linkId=None): + def draw_bounding_boxes(self, link_ids=None): """ Draw bounding box around the given link(s). Args: - linkId (int, int[N], None): link id, or list of desired link ids. If None, get links associated to + link_ids (int, int[N], None): link id, or list of desired link ids. If None, get links associated to actuated joints. """ - if linkId is None: - linkId = [-1] + self.joints - elif isinstance(linkId, int): - linkId = [linkId] + if link_ids is None: + link_ids = [-1] + self.joints + elif isinstance(link_ids, int): + link_ids = [link_ids] - for link in linkId: - if link in self.visualShapes: - bbMin, bbMax = self.sim.getAABB(self.id, link) - self._drawDebugBox(bbMin, bbMax) + for link in link_ids: + if link in self.visual_shapes: + aabb_min, aabb_max = self.sim.get_aabb(self.id, link) + self._draw_debug_box(aabb_min, aabb_max) - def draw3DEllipsoid(self, position, orientation=(0.,0.,0.,1.), scale=(1.,1.,1.), color=(0,1,0,0.7)): + def draw3d_ellipsoid(self, position, orientation=(0., 0., 0., 1.), scale=(1., 1., 1.), color=(0, 1, 0, 0.7)): """ Draw a 3D ellipsoid in the simulator. @@ -3755,15 +3163,13 @@ class Robot(object): int: id of the ellipsoid """ filename = os.path.dirname(__file__) + '/meshes/ellipsoid.obj' - visualShape = self.sim.createVisualShape(self.sim.GEOM_MESH, fileName=filename, - meshScale=scale, rgbaColor=color) - ellipsoid = self.sim.createMultiBody(baseMass=0., - baseVisualShapeIndex=visualShape, - basePosition=position, - baseOrientation=orientation) + visual_shape = self.sim.create_visual_shape(self.sim.GEOM_MESH, filename=filename, mesh_scale=scale, + rgba_color=color) + ellipsoid = self.sim.create_body(mass=0., visual_shape_id=visual_shape, position=position, + orientation=orientation) return ellipsoid - def getEllipsoidOrientationAndScale(self, X): + def get_ellipsoid_orientation_and_scale(self, X): # compute evecs and singular values _, S, V = np.linalg.svd(X) @@ -3775,7 +3181,7 @@ class Robot(object): # [np.sin(yaw), np.cos(yaw), 0], # [0, 0, 1]]).dot(np.array([0,1,0])) roll = np.arccos(v2.dot(np.array([-np.sin(yaw), np.cos(yaw), 0]))) - orientation = self.sim.getQuaternionFromEuler([roll, pitch, yaw]) + orientation = get_quaternion_from_rpy([roll, pitch, yaw]) # evals, evecs = np.linalg.eigh(X) # evals, evecs = evals[::-1], evecs[:,::-1] @@ -3797,13 +3203,13 @@ class Robot(object): return orientation, scale - def drawVelocityManipulabilityEllipsoid(self, linkId, Jlin=None, JJT=None, color=(0,1,0,0.7)): + def draw_velocity_manipulability_ellipsoid(self, link_id, Jlin=None, JJT=None, color=(0, 1, 0, 0.7)): """ evecs of JJ^T = directions singular values of JJ^T = dimensions Args: - linkId (int): link id. This will be used to check where to draw the ellipsoid. + link_id (int): link id. This will be used to check where to draw the ellipsoid. J (float[3,N], None): linear Jacobian matrix. It doesn't need to be provided if `JJT` is given. JJT (float[3,3], None): if None, it will compute it using the provided linear Jacobian matrix. @@ -3813,15 +3219,15 @@ class Robot(object): if JJT is None: if Jlin is None: raise ValueError("Please provide the linear Jacobian matrix") - JJT = self.getJJT(Jlin) + JJT = self.get_JJT(Jlin) - orientation, scale = self.getEllipsoidOrientationAndScale(JJT) + orientation, scale = self.get_ellipsoid_orientation_and_scale(JJT) # load ellipsoid - position = self.getLinkWorldPositions(linkId) - self.draw3DEllipsoid(position, orientation, scale=scale, color=color) + position = self.get_link_world_positions(link_id) + self.draw3d_ellipsoid(position, orientation, scale=scale, color=color) - def drawForceManipulabilityEllipsoid(self, linkId, J=None, JJT=None): + def draw_force_manipulability_ellipsoid(self, link_id, J=None, JJT=None): """ Kineto-statics duality: direction with good velocity manipulability is obtained a direction along which poor force manipulability is obtained. @@ -3829,26 +3235,26 @@ class Robot(object): evecs((JJ^T)^{-1}) Args: - linkId: + link_id: J: JJT: """ pass - def updateManipulabilityEllipsoid(self, linkId, ellipsoidId): + def update_manipulability_ellipsoid(self, link_id, ellipsoid_id): """ Update the position, orientation, and scaling of the given manipulability ellipsoid. Warnings: currently, the bullet simulator do not allow to update the scale, only the position and orientation. """ - raise NotImplementedError("Currently, this feature is not available on pybullet") - #self.sim.resetBasePositionAndOrientation(ellipsoidId, position, orientation) + pass + # self.sim.reset_base_pose(ellipsoid_id, position, orientation) - def removeManipulabilityEllipsoid(self, ellipsoidId): + def remove_manipulability_ellipsoid(self, ellipsoid_id): """ Remove the given ellipsoid manipulability ellipsoid. Args: - ellipsoidId (int): id of the ellipsoid to remove + ellipsoid_id (int): id of the ellipsoid to remove """ - self.sim.removeBody(ellipsoidId) + self.sim.remove_body(ellipsoid_id) diff --git a/pyrobolearn/robots/rrbot.py b/pyrobolearn/robots/rrbot.py index 8099628..d563ecd 100644 --- a/pyrobolearn/robots/rrbot.py +++ b/pyrobolearn/robots/rrbot.py @@ -17,41 +17,41 @@ class RRBot(ManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), - useFixedBase=True, + position=(0, 0, 0), + orientation=(0, 0, 0, 1), + fixed_base=True, scaling=1, - urdf_path=os.path.dirname(__file__) + '/urdfs/rrbot/rrbot.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/rrbot/rrbot.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True - super(RRBot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(RRBot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'rrbot' # set initial joint positions - self.setJointPositions(self.joints, [np.pi/4, np.pi/2]) + self.set_joint_positions(self.joints, [np.pi / 4, np.pi / 2]) for _ in range(100): - self.sim.stepSimulation() + self.sim.step() # disable each motor joint - self.disableMotor() + self.disable_motor() # self.sim.setJointMotorControlArray(self.id, self.joints, self.sim.VELOCITY_CONTROL, forces=forces) # enable F/T sensor at the end effector - self.enableJointForceTorqueSensor(2) + self.enable_joint_force_torque_sensor(2) - # Coriolis and gravity compensation (note that the setJointTorques need to be called at each time step) - self.enableCoriolisAndGravityCompensation() + # Coriolis and gravity compensation (note that the set_joint_torques need to be called at each time step) + self.enable_coriolis_and_gravity_compensation() - def getForceTorqueSensor(self, idx=0): + def get_force_torque_sensor(self, idx=0): return np.array(self.sim.getJointState(self.id, 2)[2]) @@ -69,35 +69,35 @@ if __name__ == "__main__": # load robot robot = RRBot(sim) - # robot.addJointSlider() + # robot.add_joint_slider() print("Robot: {}".format(robot)) - print("Total number of joints: {}".format(robot.getNumberOfJoints())) - print("Joint names: {}".format(robot.getJointNames(range(robot.getNumberOfJoints())))) - print("Link names: {}".format(robot.getLinkNames(range(robot.getNumberOfJoints())))) + print("Total number of joints: {}".format(robot.num_joints)) + print("Joint names: {}".format(robot.get_joint_names(range(robot.num_joints)))) + print("Link names: {}".format(robot.get_link_names(range(robot.num_joints)))) - print("Number of DoFs: {}".format(robot.getNumberOfDoFs())) + print("Number of DoFs: {}".format(robot.num_dofs)) print("Robot actuated joint ids: {}".format(robot.joints)) - print("Actuated joint names: {}".format(robot.getJointNames())) - print("Actuated link names: {}".format(robot.getLinkNames())) - print("Current joint positions: {}".format(robot.getJointPositions())) + print("Actuated joint names: {}".format(robot.get_joint_names())) + print("Actuated link names: {}".format(robot.get_link_names())) + print("Current joint positions: {}".format(robot.get_joint_positions())) - print("Number of end-effectors: {}".format(robot.getNumberOfEndEffectors())) - print("End-effector names: {}".format(robot.getEndEffectorNames())) + print("Number of end-effectors: {}".format(robot.num_end_effectors)) + print("End-effector names: {}".format(robot.get_link_names(robot.end_effectors))) - print("Robot base position: {}".format(robot.getBasePosition())) - robot.changeTransparency() - visuals = robot.sim.getVisualShapeData(robot.id) + print("Robot base position: {}".format(robot.get_base_position())) + robot.change_transparency() + visuals = robot.sim.get_visual_shape_data(robot.id) visuals = {visual[1]: visual[3] for visual in visuals} - # robot.drawLinkCoMs() - robot.drawLinkFrames() - # robot.drawBoundingBoxes() + # robot.draw_link_coms() + robot.draw_link_frames() + # robot.draw_bounding_boxes() for i in robot.joints: print("Link {}".format(i)) - state = robot.sim.getLinkState(robot.id, i) - info = robot.sim.getJointInfo(robot.id, i) + state = robot.sim.get_link_state(robot.id, i) + info = robot.sim.get_joint_info(robot.id, i) print("\t CoM world position: {}".format(state[0])) print("\t Local inertial frame position: {}".format(state[2])) @@ -116,35 +116,35 @@ if __name__ == "__main__": raw_input('press enter') - print("Inertia matrix: {}".format(np.array(sim.calculateMassMatrix(robot.id, [0.,0.,0.,0.,0.,0.])))) + print("Inertia matrix: {}".format(np.array(sim.calculate_mass_matrix(robot.id, [0., 0., 0., 0., 0., 0.])))) linkId = 2 - com_frame = robot.getLinkStates(linkId)[2] - q = robot.getJointPositions() + com_frame = robot.get_link_states(linkId)[2] + q = robot.get_joint_positions() print(com_frame) # com_frame = [0.,0.,0.] - print("Jacobian matrix: {}".format(np.vstack((sim.calculateJacobian(robot.id, linkId, com_frame, q.tolist(), [0.,0.], [0.,0.]))))) + print("Jacobian matrix: {}".format(sim.calculate_jacobian(robot.id, linkId, com_frame))) - Jlin = robot.calculateJacobian(linkId+1, localPosition=(0.,0.,0.))[:3] + Jlin = robot.get_jacobian(linkId + 1)[:3] print("Jacobian matrix: {}".format(Jlin)) - robot.drawVelocityManipulabilityEllipsoid(linkId+1, Jlin) + robot.draw_velocity_manipulability_ellipsoid(linkId + 1, Jlin) - Jlin = robot.calculateJacobian(linkId)[:3] - robot.drawVelocityManipulabilityEllipsoid(linkId, Jlin, color=(1,0,0,0.7)) + Jlin = robot.get_jacobian(linkId)[:3] + robot.draw_velocity_manipulability_ellipsoid(linkId, Jlin, color=(1, 0, 0, 0.7)) cnt = 0 for i in count(): if i%240 == 0: if cnt < 3: - Jlin = robot.calculateJacobian(linkId + 1, localPosition=(0., 0., 0.))[:3] - robot.drawVelocityManipulabilityEllipsoid(linkId + 1, Jlin) + Jlin = robot.get_jacobian(linkId + 1)[:3] + robot.draw_velocity_manipulability_ellipsoid(linkId + 1, Jlin) cnt += 1 world.step(sleep_dt=1./240) - # robot.setJointTorques() + # robot.set_joint_torques() raw_input('press enter') - print(robot.getLinkNames()) + print(robot.get_link_names()) force = np.array([1., 0., 0.]) pos = np.array([0., 0., 0.]) sim.applyExternalForce(robot.id, 1, force, pos, flags=p.LINK_FRAME) # link_frame = 1 @@ -152,32 +152,32 @@ if __name__ == "__main__": slider = sim.addUserDebugParameter('force', -1000., 1000., 0) dq, ddq = [0., 0.], [0., 0.] - J = sim.calculateJacobian(robot.id, 1, [0.,0.,0.], [0.,0.], dq, ddq) + J = sim.get_jacobian(robot.id, 1, [0., 0., 0.]) print(np.array(J[0])) - a = robot.getJointPositions() + a = robot.get_joint_positions() # print(robot.getJacobianMatrix(1, np.array([0.,0.]))) # TODO: need to convert numpy array to list linkId = 2 com_frame = robot.getLinkStates(linkId)[2] - xdes = np.array(robot.getLinkWorldPositions(linkId)) + xdes = np.array(robot.get_link_world_positions(linkId)) K = 100*np.identity(3) D = 2*np.sqrt(K) # critically damped D = 3*D # manually increase damping # run simulator for i in range(10000): - joint_states = p.getJointStates(robot.id, robot.joints) + joint_states = p.get_joint_states(robot.id, robot.joints) # print("joint state: ", joint_states) q = [joint_state[0] for joint_state in joint_states] dq = [joint_state[1] for joint_state in joint_states] - # q = robot.getJointPositions().tolist() - # dq = robot.getJointVelocities().tolist() - x = np.array(robot.getLinkWorldPositions(linkId)) - dx = np.array(robot.getLinkWorldLinearVelocities(linkId)) + # q = robot.get_joint_positions().tolist() + # dq = robot.get_joint_velocities().tolist() + x = np.array(robot.get_link_world_positions(linkId)) + dx = np.array(robot.get_link_world_linear_velocities(linkId)) tau = robot.calculateID(q, dq, ddq) # Coriolis, centrifugal and gravity compensation - Jlin = np.array(sim.calculateJacobian(robot.id, linkId, com_frame, q, [0.,0.], ddq)[0]) + Jlin = np.array(sim.get_jacobian(robot.id, linkId, com_frame)[0]) F = K.dot(xdes - x) - D.dot(dx) # compute cartesian forces # print("force: {}".format(F)) tau += Jlin.T.dot(F) # cartesian PD with gravity compensation @@ -200,5 +200,5 @@ if __name__ == "__main__": force = np.array([0., 0., 0.]) sim.applyExternalForce(robot.id, linkId, force, pos, flags=p.LINK_FRAME) # p.LINK_FRAME = 1 - # robot.updateJointSlider() + # robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/sawyer.py b/pyrobolearn/robots/sawyer.py index 7fa5e1e..8ec0311 100644 --- a/pyrobolearn/robots/sawyer.py +++ b/pyrobolearn/robots/sawyer.py @@ -19,30 +19,30 @@ class Sawyer(ManipulatorRobot, WheeledRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.92), - init_orient=(0, 0, 0, 1), - useFixedBase=True, + position=(0, 0, 0.92), + orientation=(0, 0, 0, 1), + fixed_base=True, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/sawyer/sawyer.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/sawyer/sawyer.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.92) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.92,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.92) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.92,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True - super(Sawyer, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Sawyer, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'sawyer' - self.head = self.getLinkIds('head') if 'head' in self.link_names else None + self.head = self.get_link_ids('head') if 'head' in self.link_names else None - self.arms = [[self.getLinkIds(link) for link in ['right_l0', 'right_l1', 'right_l2', 'right_l3', 'right_l4', + self.arms = [[self.get_link_ids(link) for link in ['right_l0', 'right_l1', 'right_l2', 'right_l3', 'right_l4', 'right_l5', 'right_l6'] if link in self.link_names]] - self.hands = [self.getLinkIds(link) for link in ['right_l6'] if link in self.link_names] + self.hands = [self.get_link_ids(link) for link in ['right_l6'] if link in self.link_names] # Test @@ -61,12 +61,12 @@ if __name__ == "__main__": robot = Sawyer(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # # Position control using sliders - robot.addJointSlider() + robot.add_joint_slider() # run simulator for _ in count(): - robot.updateJointSlider() + robot.update_joint_slider() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/sea_hexapod.py b/pyrobolearn/robots/sea_hexapod.py index a94282d..fffbdbf 100644 --- a/pyrobolearn/robots/sea_hexapod.py +++ b/pyrobolearn/robots/sea_hexapod.py @@ -16,22 +16,22 @@ class SEAHexapod(HexapodRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.15), - init_orient=(0, 0, 0.707, 0.707), - useFixedBase=False, + position=(0, 0, 0.15), + orientation=(0, 0, 0.707, 0.707), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/cmu_sea/hexapod.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/cmu_sea/hexapod.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.15) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.15,) - if init_orient is None: - init_orient = (0, 0, 0.707, 0.707) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.15) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.15,) + if orientation is None: + orientation = (0, 0, 0.707, 0.707) + if fixed_base is None: + fixed_base = False - super(SEAHexapod, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(SEAHexapod, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'sea_hexapod' @@ -51,7 +51,7 @@ if __name__ == "__main__": robot = SEAHexapod(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # run simulation for i in count(): diff --git a/pyrobolearn/robots/sea_snake.py b/pyrobolearn/robots/sea_snake.py index 76c2450..07fc87f 100644 --- a/pyrobolearn/robots/sea_snake.py +++ b/pyrobolearn/robots/sea_snake.py @@ -16,22 +16,22 @@ class SEASnake(Robot): def __init__(self, simulator, - init_pos=(-0.5, 0, 0.1), - init_orient=(0, 0.707, 0, 0.707), - useFixedBase=False, + position=(-0.5, 0, 0.1), + orientation=(0, 0.707, 0, 0.707), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/cmu_sea/snake.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/cmu_sea/snake.urdf'): # check parameters - if init_pos is None: - init_pos = (-0.5, 0., 0.1) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.1,) - if init_orient is None: - init_orient = (0, 0.707, 0, 0.707) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (-0.5, 0., 0.1) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.1,) + if orientation is None: + orientation = (0, 0.707, 0, 0.707) + if fixed_base is None: + fixed_base = False - super(SEASnake, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(SEASnake, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'sea_snake' @@ -51,7 +51,7 @@ if __name__ == "__main__": robot = SEASnake(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # run simulation for i in count(): diff --git a/pyrobolearn/robots/softhand.py b/pyrobolearn/robots/softhand.py index 19c128b..7f892e4 100644 --- a/pyrobolearn/robots/softhand.py +++ b/pyrobolearn/robots/softhand.py @@ -18,29 +18,29 @@ class SoftHand(Hand): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), + position=(0, 0, 0), + orientation=(0, 0, 0, 1), scaling=1., left=True, - useFixedBase=True): + fixed_base=True): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if fixed_base is None: + fixed_base = True if left: - if init_orient is None: - init_orient = (0, 0, 0, 1) + if orientation is None: + orientation = (0, 0, 0, 1) urdf_path = os.path.dirname(__file__) + '/urdfs/softhand/left_hand.urdf' else: - if init_orient is None: - init_orient = (0, 0, 1, 0) + if orientation is None: + orientation = (0, 0, 1, 0) urdf_path = os.path.dirname(__file__) + '/urdfs/softhand/right_hand.urdf' - super(SoftHand, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(SoftHand, self).__init__(simulator, urdf_path, position, orientation, fixed_base, scaling) self.name = 'soft_hand' @@ -57,23 +57,23 @@ if __name__ == "__main__": world = BasicWorld(sim) # create robot - left_hand = SoftHand(sim, init_pos=(-0.15, 0, 0), left=True) - right_hand = SoftHand(sim, init_pos=(0.15, 0., 0.), init_orient=(0, 0, 1, 0), left=False) + left_hand = SoftHand(sim, position=(-0.15, 0, 0), left=True) + right_hand = SoftHand(sim, position=(0.15, 0., 0.), orientation=(0, 0, 1, 0), left=False) # print information about the robot - left_hand.printRobotInfo() - # H = left_hand.calculateMassMatrix() + left_hand.print_info() + # H = left_hand.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) # Position control using sliders - # left_hand.addJointSlider() + # left_hand.add_joint_slider() - left_hand.setJointPositions([0.] * left_hand.getNumberOfDoFs()) - right_hand.setJointPositions([0.] * right_hand.getNumberOfDoFs()) + left_hand.set_joint_positions([0.] * left_hand.getNumberOfDoFs()) + right_hand.set_joint_positions([0.] * right_hand.getNumberOfDoFs()) for i in count(): - # left_hand.updateJointSlider() - # left_hand.setJointPositions([0.] * left_hand.getNumberOfDoFs()) - # right_hand.setJointPositions([0.] * right_hand.getNumberOfDoFs()) + # left_hand.update_joint_slider() + # left_hand.set_joint_positions([0.] * left_hand.getNumberOfDoFs()) + # right_hand.set_joint_positions([0.] * right_hand.getNumberOfDoFs()) world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/swimmer.py b/pyrobolearn/robots/swimmer.py index 9cfff9c..271a03b 100644 --- a/pyrobolearn/robots/swimmer.py +++ b/pyrobolearn/robots/swimmer.py @@ -13,22 +13,22 @@ class Swimmer(Robot): def __init__(self, simulator, - init_pos=(-0.5, 0, 0.1), - init_orient=(0, 0.707, 0, 0.707), - useFixedBase=False, + position=(-0.5, 0, 0.1), + orientation=(0, 0.707, 0, 0.707), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/mjcfs/swimmer.xml'): + urdf=os.path.dirname(__file__) + '/mjcfs/swimmer.xml'): # check parameters - if init_pos is None: - init_pos = (-0.5, 0., 0.1) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.1,) - if init_orient is None: - init_orient = (0, 0.707, 0, 0.707) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (-0.5, 0., 0.1) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.1,) + if orientation is None: + orientation = (0, 0.707, 0, 0.707) + if fixed_base is None: + fixed_base = False - super(Swimmer, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Swimmer, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'swimmer' @@ -48,7 +48,7 @@ if __name__ == "__main__": robot = Swimmer(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # run simulation for i in count(): diff --git a/pyrobolearn/robots/uav.py b/pyrobolearn/robots/uav.py index 521a147..639410f 100644 --- a/pyrobolearn/robots/uav.py +++ b/pyrobolearn/robots/uav.py @@ -11,9 +11,8 @@ class UAVRobot(Robot): Vehicles/Robots that operate in the air. These are also called drones. """ - def __init__(self, simulator, urdf_path, init_pos=(0, 0, 1.), init_orient=(0, 0, 0, 1), useFixedBase=False, - scaling=1.): - super(UAVRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.): + super(UAVRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.propellers = [] # list of propellers id @@ -28,9 +27,8 @@ class FixedWingUAV(UAVRobot): """ - def __init__(self, simulator, urdf_path, init_pos=(0, 0, 1.), init_orient=(0, 0, 0, 1), useFixedBase=False, - scaling=1.): - super(FixedWingUAV, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.): + super(FixedWingUAV, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) class RotaryWingUAV(UAVRobot): @@ -38,6 +36,5 @@ class RotaryWingUAV(UAVRobot): """ - def __init__(self, simulator, urdf_path, init_pos=(0, 0, 1.), init_orient=(0, 0, 0, 1), useFixedBase=False, - scaling=1.): - super(RotaryWingUAV, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.): + super(RotaryWingUAV, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) diff --git a/pyrobolearn/robots/usv.py b/pyrobolearn/robots/usv.py index 4067874..75831bc 100644 --- a/pyrobolearn/robots/usv.py +++ b/pyrobolearn/robots/usv.py @@ -11,6 +11,5 @@ class USVRobot(Robot): Vehicles/Robots that operate on the surface of water. """ - def __init__(self, simulator, urdf_path, init_pos=(0, 0, 1.), init_orient=(0, 0, 0, 1), useFixedBase=False, - scaling=1.): - super(USVRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.): + super(USVRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) diff --git a/pyrobolearn/robots/uuv.py b/pyrobolearn/robots/uuv.py index f30cb48..dc3a9f3 100644 --- a/pyrobolearn/robots/uuv.py +++ b/pyrobolearn/robots/uuv.py @@ -11,6 +11,5 @@ class UUVRobot(Robot): Vehicles/Robots that operate under water. """ - def __init__(self, simulator, urdf_path, init_pos=(0, 0, 1.), init_orient=(0, 0, 0, 1), useFixedBase=False, - scaling=1.): - super(UUVRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.): + super(UUVRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) diff --git a/pyrobolearn/robots/walker2d.py b/pyrobolearn/robots/walker2d.py index 47714f0..49863fc 100644 --- a/pyrobolearn/robots/walker2d.py +++ b/pyrobolearn/robots/walker2d.py @@ -13,22 +13,22 @@ class Walker2D(Robot): def __init__(self, simulator, - init_pos=(-0.5, 0, 0.1), - init_orient=(0, 0.707, 0, 0.707), - useFixedBase=False, + position=(-0.5, 0, 0.1), + orientation=(0, 0.707, 0, 0.707), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/mjcfs/walker2d.xml'): + urdf=os.path.dirname(__file__) + '/mjcfs/walker2d.xml'): # check parameters - if init_pos is None: - init_pos = (-0.5, 0., 0.1) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.1,) - if init_orient is None: - init_orient = (0, 0.707, 0, 0.707) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (-0.5, 0., 0.1) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.1,) + if orientation is None: + orientation = (0, 0.707, 0, 0.707) + if fixed_base is None: + fixed_base = False - super(Walker2D, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Walker2D, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'walker2D' @@ -48,7 +48,7 @@ if __name__ == "__main__": robot = Walker2D(sim) # print information about the robot - robot.printRobotInfo() + robot.print_info() # run simulation for i in count(): diff --git a/pyrobolearn/robots/walkman.py b/pyrobolearn/robots/walkman.py index 3740970..2ace4a5 100644 --- a/pyrobolearn/robots/walkman.py +++ b/pyrobolearn/robots/walkman.py @@ -22,25 +22,25 @@ class Walkman(BipedRobot, BiManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 1.14), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 1.14), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/walkman/walkman.urdf', + urdf=os.path.dirname(__file__) + '/urdfs/walkman/walkman.urdf', lower_body=False): # 'walkman_lower_body.urdf' # check parameters - if init_pos is None: - init_pos = (0., 0., 1.14) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (1.14,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 1.14) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (1.14,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False if lower_body: - urdf_path = os.path.dirname(__file__) + '/urdfs/walkman/walkman_lower_body.urdf' + urdf = os.path.dirname(__file__) + '/urdfs/walkman/walkman_lower_body.urdf' - super(Walkman, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Walkman, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'walkman' # Camera sensors: Two 2D Camera sensor (stereo-camera) @@ -66,27 +66,27 @@ class Walkman(BipedRobot, BiManipulatorRobot): # IMU sensors # End-effectors (arms and legs) - self.waist = [self.getLinkIds(link) for link in ['DWL', 'DWS', 'DWYTorso'] if link in self.link_names] - self.neck = [self.getLinkIds(link) for link in ['NeckYaw', 'NeckPitch'] if link in self.link_names] + self.waist = [self.get_link_ids(link) for link in ['DWL', 'DWS', 'DWYTorso'] if link in self.link_names] + self.neck = [self.get_link_ids(link) for link in ['NeckYaw', 'NeckPitch'] if link in self.link_names] - self.legs = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['LHipMot', 'LThighUpLeg', 'LThighLowLeg', 'LLowLeg', 'LFootmot', 'LFoot'], ['RHipMot', 'RThighUpLeg', 'RThighLowLeg', 'RLowLeg', 'RFootmot', 'RFoot']]] - self.feet = [self.getLinkIds(link) for link in ['LFoot', 'RFoot'] if link in self.link_names] + self.feet = [self.get_link_ids(link) for link in ['LFoot', 'RFoot'] if link in self.link_names] - self.arms = [[self.getLinkIds(link) for link in links if link in self.link_names] + self.arms = [[self.get_link_ids(link) for link in links if link in self.link_names] for links in [['LShp', 'LShr', 'LShy', 'LElb', 'LForearm', 'LWrMot2', 'LWrMot3'], ['RShp', 'RShr', 'RShy', 'RElb', 'RForearm', 'RWrMot2', 'RWrMot3']]] - self.hands = [self.getLinkIds(link) for link in ['LSoftHand', 'RSoftHand'] if link in self.link_names] + self.hands = [self.get_link_ids(link) for link in ['LSoftHand', 'RSoftHand'] if link in self.link_names] def WalkmanLowerBody(simulator, init_pos=(0, 0, 1.5), init_orient=(0, 0, 0, 1), useFixedBase=False, scaling=1., urdf_path=os.path.dirname(__file__) + '/urdfs/walkman/walkman_lower_body.urdf'): """Load Walkman Lower Body""" - return Walkman(simulator=simulator, init_pos=init_pos, init_orient=init_orient, useFixedBase=useFixedBase, - scaling=scaling, urdf_path=urdf_path) + return Walkman(simulator=simulator, position=init_pos, orientation=init_orient, fixed_base=useFixedBase, + scaling=scaling, urdf=urdf_path) # Test @@ -105,18 +105,18 @@ if __name__ == "__main__": world.loadSphere([2., 1., 2.], mass=0., color=(0, 0, 1, 1)) # load robot - robot = Walkman(sim, useFixedBase=False, lower_body=False) + robot = Walkman(sim, fixed_base=False, lower_body=False) # print information about the robot - robot.printRobotInfo() + robot.print_info() # # Position control using sliders - robot.addJointSlider(robot.left_leg) + robot.add_joint_slider(robot.left_leg) # run simulator for i in count(): - robot.updateJointSlider() + robot.update_joint_slider() if i % 60 == 0: - img = robot.left_camera.getRGBImage() + img = robot.left_camera.get_rgb_image() world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/wam.py b/pyrobolearn/robots/wam.py index 2067d3e..2d4d625 100644 --- a/pyrobolearn/robots/wam.py +++ b/pyrobolearn/robots/wam.py @@ -16,25 +16,25 @@ class WAM(ManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0), - init_orient=(0, 0, 0, 1), - useFixedBase=True, + position=(0, 0, 0), + orientation=(0, 0, 0, 1), + fixed_base=True, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/wam/wam.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/wam/wam.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True - super(WAM, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(WAM, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'wam' - # self.disableMotor() + # self.disable_motor() # Test @@ -54,29 +54,29 @@ if __name__ == "__main__": robot = WAM(sim) # print information about the robot - robot.printRobotInfo() - # H = robot.calculateMassMatrix() + robot.print_info() + # H = robot.get_mass_matrix() # print("Inertia matrix: H(q) = {}".format(H)) - robot.setJointPositions([np.pi / 4, np.pi / 2], jointId=[0,1]) #2, 4]) + robot.set_joint_positions([np.pi / 4, np.pi / 2], joint_ids=[0, 1]) #2, 4]) - Jlin = robot.calculateJacobian(6, localPosition=(0., 0., 0.))[:3] - robot.drawVelocityManipulabilityEllipsoid(6, Jlin, color=(1,0,0,0.7)) + Jlin = robot.get_jacobian(6)[:3] + robot.draw_velocity_manipulability_ellipsoid(6, Jlin, color=(1, 0, 0, 0.7)) for _ in range(5): world.step(sleep_dt=1./240) - Jlin = robot.calculateJacobian(6, localPosition=(0., 0., 0.))[:3] - robot.drawVelocityManipulabilityEllipsoid(6, Jlin, color=(0, 0, 1, 0.7)) + Jlin = robot.get_jacobian(6)[:3] + robot.draw_velocity_manipulability_ellipsoid(6, Jlin, color=(0, 0, 1, 0.7)) for _ in range(45): world.step(sleep_dt=1./240) - Jlin = robot.calculateJacobian(6, localPosition=(0., 0., 0.))[:3] - robot.drawVelocityManipulabilityEllipsoid(6, Jlin) + Jlin = robot.get_jacobian(6)[:3] + robot.draw_velocity_manipulability_ellipsoid(6, Jlin) for i in count(): if i%1000 == 0: - print("Joint Torques: {}".format(robot.getJointTorques())) - print("Gravity Torques: {}".format(robot.getGravityCompensationTorques())) - print("Compensation Torques: {}".format(robot.getCoriolisAndGravityCompensationTorques())) + print("Joint Torques: {}".format(robot.get_joint_torques())) + print("Gravity Torques: {}".format(robot.get_gravity_compensation_torques())) + print("Compensation Torques: {}".format(robot.get_coriolis_and_gravity_compensation_torques())) # step in simulation world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/wheeled_robot.py b/pyrobolearn/robots/wheeled_robot.py index f104315..c92d3cd 100644 --- a/pyrobolearn/robots/wheeled_robot.py +++ b/pyrobolearn/robots/wheeled_robot.py @@ -8,10 +8,13 @@ from pyrobolearn.robots.robot import Robot class WheeledRobot(Robot): + r"""Wheeled robot - def __init__(self, simulator, urdf_path, init_pos=(0, 0, 1.), init_orient=(0, 0, 0, 1), useFixedBase=False, - scaling=1.): - super(WheeledRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + This type of robots has wheels. + """ + + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.): + super(WheeledRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.wheels = [] self.wheel_directions = [] @@ -22,23 +25,14 @@ class WheeledRobot(Robot): @property def num_wheels(self): - """Return the number of wheels""" + """Return the number of wheels.""" return len(self.wheels) ########### # Methods # ########### - def getNumberOfWheels(self): - """ - Return the number of wheels. - - Returns: - int: the number of wheels. - """ - return self.num_wheels - - def getWheelIds(self, wheels=None): + def get_wheel_ids(self, wheels=None): """ Return the wheel id associated with the given wheel number(s)/name(s). @@ -48,46 +42,42 @@ class WheeledRobot(Robot): Returns: list[int]: wheel id(s) """ - if wheels: + if wheels is not None: if isinstance(wheels, int): return self.wheels[wheels] elif isinstance(wheels, str): - return self.wheels[self.getLinkIds(wheels)] + return self.wheels[self.get_link_ids(wheels)] elif isinstance(wheels, (list, tuple)): - wheelIds = [] + wheel_ids = [] for wheel in wheels: - if isinstance(wheels, int): - wheelIds.append(self.wheels[wheels]) - elif isinstance(wheels, str): - wheelIds.append(self.wheels[self.getLinkIds(wheels)]) + if isinstance(wheel, int): + wheel_ids.append(self.wheels[wheel]) + elif isinstance(wheel, str): + wheel_ids.append(self.wheels[self.get_link_ids(wheel)]) else: raise TypeError("Expecting a str or int for items in wheels") - return wheelIds + return wheel_ids return self.wheels - def getWheelNames(self, wheelId=None): - """Return the wheel names associated to the given ids""" - return self.getLinkNames(wheelId) - def drive(self, speed): if isinstance(speed, (int, float)): speed = speed * np.ones(self.num_wheels) speed = speed * self.wheel_directions - self.setJointVelocities(speed, self.wheels) + self.set_joint_velocities(speed, self.wheels) def stop(self): - self.setJointVelocities(np.zeros(self.num_wheels), self.wheels) + self.set_joint_velocities(np.zeros(self.num_wheels), self.wheels) - def driveForward(self, speed): + def drive_forward(self, speed): self.drive(speed) - def driveBackward(self, speed): + def drive_backward(self, speed): self.drive(-speed) - def turnRight(self): + def turn_right(self): pass - def turnLeft(self): + def turn_left(self): pass @@ -109,9 +99,8 @@ class DifferentialWheeledRobot(WheeledRobot): http://www.robotplatform.com/knowledge/Classification_of_Robots/wheel_control_theory.html """ - def __init__(self, simulator, urdf_path, init_pos=(0, 0, 1.), init_orient=(0, 0, 0, 1), useFixedBase=False, - scaling=1.): - super(DifferentialWheeledRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.): + super(DifferentialWheeledRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) @@ -131,13 +120,13 @@ class AckermannWheeledRobot(WheeledRobot): [3] Wheel Control Theory: http://www.robotplatform.com/knowledge/Classification_of_Robots/wheel_control_theory.html """ - def __init__(self, simulator, urdf_path, init_pos=(0, 0, 1.), init_orient=(0, 0, 0, 1), useFixedBase=False, - scaling=1.): - super(AckermannWheeledRobot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, + + def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.): + super(AckermannWheeledRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.steering = 0 # id of steering joint - def setSteering(self, angle): + def set_steering(self, angle): """Set steering angle""" pass diff --git a/pyrobolearn/robots/youbot.py b/pyrobolearn/robots/youbot.py index 16eaa58..92d4679 100644 --- a/pyrobolearn/robots/youbot.py +++ b/pyrobolearn/robots/youbot.py @@ -20,25 +20,25 @@ class YoubotBase(DifferentialWheeledRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.085), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.085), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/youbot/youbot_base_only.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/youbot/youbot_base_only.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.085) - if len(init_pos) == 2: # assume x, y are given - init_pos = tuple(init_pos) + (0.085,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.085) + if len(position) == 2: # assume x, y are given + position = tuple(position) + (0.085,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(YoubotBase, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(YoubotBase, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'youbot_base' - # self.wheels = [self.getLinkIds(link) for link in ['left_wheel', 'right_wheel'] + # self.wheels = [self.get_link_ids(link) for link in ['left_wheel', 'right_wheel'] # if link in self.link_names] # self.wheel_directions = np.ones(len(self.wheels)) @@ -52,22 +52,22 @@ class KukaYoubotArm(ManipulatorRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.03), - init_orient=(0, 0, 0, 1), - useFixedBase=True, + position=(0, 0, 0.03), + orientation=(0, 0, 0, 1), + fixed_base=True, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/youbot/youbot_arm_only.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/youbot/youbot_arm_only.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.03) - if len(init_pos) == 2: # assume x, y are given - init_pos += (0.03,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = True + if position is None: + position = (0., 0., 0.03) + if len(position) == 2: # assume x, y are given + position += (0.03,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = True - super(KukaYoubotArm, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(KukaYoubotArm, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'kuka_youbot_arm' @@ -80,25 +80,25 @@ class Youbot(ManipulatorRobot, DifferentialWheeledRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.085), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.085), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/youbot/youbot.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/youbot/youbot.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.085) - if len(init_pos) == 2: # assume x, y are given - init_pos += (0.085,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.085) + if len(position) == 2: # assume x, y are given + position += (0.085,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(Youbot, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(Youbot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'youbot' - # self.wheels = [self.getLinkIds(link) for link in ['left_wheel', 'right_wheel'] + # self.wheels = [self.get_link_ids(link) for link in ['left_wheel', 'right_wheel'] # if link in self.link_names] # self.wheel_directions = np.ones(len(self.wheels)) @@ -112,25 +112,25 @@ class YoubotDualArm(BiManipulatorRobot, DifferentialWheeledRobot): def __init__(self, simulator, - init_pos=(0, 0, 0.085), - init_orient=(0, 0, 0, 1), - useFixedBase=False, + position=(0, 0, 0.085), + orientation=(0, 0, 0, 1), + fixed_base=False, scaling=1., - urdf_path=os.path.dirname(__file__) + '/urdfs/youbot/youbot_dual_arm.urdf'): + urdf=os.path.dirname(__file__) + '/urdfs/youbot/youbot_dual_arm.urdf'): # check parameters - if init_pos is None: - init_pos = (0., 0., 0.085) - if len(init_pos) == 2: # assume x, y are given - init_pos += (0.085,) - if init_orient is None: - init_orient = (0, 0, 0, 1) - if useFixedBase is None: - useFixedBase = False + if position is None: + position = (0., 0., 0.085) + if len(position) == 2: # assume x, y are given + position += (0.085,) + if orientation is None: + orientation = (0, 0, 0, 1) + if fixed_base is None: + fixed_base = False - super(YoubotDualArm, self).__init__(simulator, urdf_path, init_pos, init_orient, useFixedBase, scaling) + super(YoubotDualArm, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling) self.name = 'youbot_dual_arm' - # self.wheels = [self.getLinkIds(link) for link in ['left_wheel', 'right_wheel'] + # self.wheels = [self.get_link_ids(link) for link in ['left_wheel', 'right_wheel'] # if link in self.link_names] # self.wheel_directions = np.ones(len(self.wheels)) @@ -148,23 +148,23 @@ if __name__ == "__main__": world = BasicWorld(sim) # create robot - youbot_base = YoubotBase(sim, init_pos=(0, -0.75)) - kuka_arm = KukaYoubotArm(sim, init_pos=(0, -0.25)) - youbot = Youbot(sim, init_pos=(0, 0.25)) - youbot_dual_arm = YoubotDualArm(sim, init_pos=(0, 0.75)) + youbot_base = YoubotBase(sim, position=(0, -0.75)) + kuka_arm = KukaYoubotArm(sim, position=(0, -0.25)) + youbot = Youbot(sim, position=(0, 0.25)) + youbot_dual_arm = YoubotDualArm(sim, position=(0, 0.75)) robots = [youbot_base, kuka_arm, youbot, youbot_dual_arm] # print information about the robot for robot in robots: - robot.printRobotInfo() + robot.print_info() # Position control using sliders - # robot.addJointSlider() + # robot.add_joint_slider() # run simulator for _ in count(): - # robots[0].updateJointSlider() + # robots[0].update_joint_slider() # for robot in robots: # robot.drive(5) world.step(sleep_dt=1./240) diff --git a/pyrobolearn/simulators/__init__.py b/pyrobolearn/simulators/__init__.py index aae9ae0..e915ed0 100644 --- a/pyrobolearn/simulators/__init__.py +++ b/pyrobolearn/simulators/__init__.py @@ -5,18 +5,18 @@ from .simulator import Simulator # bullet simulator -from .bullet import Bullet +from .bullet import Bullet as BulletSim -# PyBullet simulator -import pybullet -import pybullet_data -from pybullet_envs.bullet.bullet_client import BulletClient - - -def BulletSim(mode=pybullet.GUI, debug_visualizer=False): - """mode: pybullet.GUI, pybullet.DIRECT""" - sim = BulletClient(connection_mode=mode) - sim.setAdditionalSearchPath(pybullet_data.getDataPath()) - if not debug_visualizer: - sim.configureDebugVisualizer(sim.COV_ENABLE_GUI, 0) - return sim +# # PyBullet simulator +# import pybullet +# import pybullet_data +# from pybullet_envs.bullet.bullet_client import BulletClient +# +# +# def BulletSim(mode=pybullet.GUI, debug_visualizer=False): +# """mode: pybullet.GUI, pybullet.DIRECT""" +# sim = BulletClient(connection_mode=mode) +# sim.setAdditionalSearchPath(pybullet_data.getDataPath()) +# if not debug_visualizer: +# sim.configureDebugVisualizer(sim.COV_ENABLE_GUI, 0) +# return sim diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 109e2cd..8890f55 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -83,6 +83,7 @@ class Bullet(Simulator): # Connect to pybullet if render: self.sim = BulletClient(connection_mode=pybullet.GUI) + self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_GUI, 0) else: self.sim = BulletClient(connection_mode=pybullet.DIRECT) self.id = self.sim._client @@ -90,10 +91,11 @@ class Bullet(Simulator): # add additional search path when loading URDFs, SDFs, MJCFs, etc. self.sim.setAdditionalSearchPath(pybullet_data.getDataPath()) - # Converters - # if converter is None: - self.conv = NumpyListConverter() - self.quat_conv = QuaternionListConverter(convention=1) + # go through the global variables / attributes defined in pybullet and set them here + # this includes for instance: JOINT_REVOLUTE, POSITION_CONTROL, etc. + # for attribute in dir(pybullet): + # if attribute[0].isupper(): # any global variable starts with a capital letter + # setattr(self, attribute, getattr(pybullet, attribute)) # def __del__(self): # """Clean up connection if not already done. @@ -391,7 +393,7 @@ class Bullet(Simulator): """ self.sim.setGravity(gravity[0], gravity[1], gravity[2]) - def save(self, filename=None): + def save(self, filename=None, *args, **kwargs): """ Save the state of the simulator. @@ -400,13 +402,14 @@ class Bullet(Simulator): memory instead of the disk. Returns: - int: unique state id. This id can be used to load the state. + int / str: unique state id, or filename. This id / filename can be used to load the state. """ if filename is None: return self.sim.saveState() - return self.sim.saveBullet(filename) + self.sim.saveBullet(filename) + return filename - def load(self, state): + def load(self, state, *args, **kwargs): """ Load/Restore the simulator to a previous state. @@ -418,7 +421,7 @@ class Bullet(Simulator): elif isinstance(state, str): self.sim.restoreState(fileName=state) - def load_plugin(self, plugin_path, name): + def load_plugin(self, plugin_path, name, *args, **kwargs): """Load a certain plugin in the simulator. Few examples can be found at: https://github.com/bulletphysics/bullet3/tree/master/examples/SharedMemory/plugins @@ -451,7 +454,7 @@ class Bullet(Simulator): kwargs.setdefault('floatArgs', []).append(arg) self.sim.executePluginCommand(plugin_id, **kwargs) - def unload_plugin(self, plugin_id): + def unload_plugin(self, plugin_id, *args, **kwargs): """Unload the specified plugin from the simulator. Args: @@ -463,11 +466,11 @@ class Bullet(Simulator): # loading URDFs, SDFs, MJCFs, meshes # ###################################### - def load_urdf(self, filename, position=None, orientation=None, use_maximal_coordinates=0, - use_fixed_base=0, flags=0, scale=1.0): + def load_urdf(self, filename, position=None, orientation=None, use_maximal_coordinates=None, + use_fixed_base=None, flags=None, scale=None): """Load the given URDF file. - The loadURDF will send a command to the physics server to load a physics model from a Universal Robot + The load_urdf will send a command to the physics server to load a physics model from a Universal Robot Description File (URDF). The URDF file is used by the ROS project (Robot Operating System) to describe robots and other objects, it was created by the WillowGarage and the Open Source Robotics Foundation (OSRF). Many robots have public URDF files, you can find a description and tutorial here: @@ -509,22 +512,30 @@ class Bullet(Simulator): Returns: int (non-negative): unique id associated to the load model. """ + kwargs = {} if position is not None: if isinstance(position, np.ndarray): position = position.ravel().tolist() + kwargs['basePosition'] = position if orientation is not None: if isinstance(orientation, np.ndarray): orientation = orientation.ravel().tolist() - elif isinstance(orientation, quaternion.quaternion): - orientation = self.quat_conv.convertFrom(orientation) + kwargs['baseOrientation'] = orientation + if use_maximal_coordinates is not None: + kwargs['useMaximalCoordinates'] = use_maximal_coordinates + if use_fixed_base is not None: + kwargs['useFixedBase'] = use_fixed_base + if flags is not None: + kwargs['flags'] = flags + if scale is not None: + kwargs['globalScaling'] = scale - return self.sim.loadURDF(filename, position, orientation, use_maximal_coordinates, int(use_fixed_base), flags, - scale) + return self.sim.loadURDF(filename, **kwargs) - def load_sdf(self, filename, scaling=1.): + def load_sdf(self, filename, scaling=1., *args, **kwargs): """Load the given SDF file. - The loadSDF command only extracts some essential parts of the SDF related to the robot models and geometry, + The load_sdf command only extracts some essential parts of the SDF related to the robot models and geometry, and ignores many elements related to cameras, lights and so on. Args: @@ -536,10 +547,10 @@ class Bullet(Simulator): """ return self.sim.loadSDF(filename, globalScaling=scaling) - def load_mjcf(self, filename, scaling=1.): + def load_mjcf(self, filename, scaling=1., *args, **kwargs): """Load the given MJCF file. - "The loadMJCF command performs basic import of MuJoCo MJCF xml files, used in OpenAI Gym". [1] + "The load_mjcf command performs basic import of MuJoCo MJCF xml files, used in OpenAI Gym". [1] It will load all the object described in a MJCF file. Args: @@ -549,10 +560,10 @@ class Bullet(Simulator): Returns: list(int): list of object unique id for each object loaded """ - return self.sim.loadMJCF(filename, globalScaling=scaling) + return self.sim.loadMJCF(filename) def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), - color=None, flags=None, *args, **kwargs): + color=None, with_collision=True, flags=None, *args, **kwargs): """ Load a mesh in the world (only available in the simulator). @@ -565,6 +576,7 @@ class Bullet(Simulator): mass (float): mass of the mesh (in kg). If mass = 0, it won't move even if there is a collision. scale (float[3]): scale the mesh in the (x,y,z) directions color (int[4], None): color of the mesh (by default: white and opaque) + with_collision (bool): If True, it will also create the collision mesh, and not only a visual mesh. flags (int, None): if flag = `sim.GEOM_FORCE_CONCAVE_TRIMESH` (=1), this will create a concave static triangle mesh. This should not be used with dynamic/moving objects, only for static (mass=0) terrain. @@ -575,9 +587,11 @@ class Bullet(Simulator): if flags is not None: kwargs['flags'] = flags - # create collision shape - collision_shape = self.sim.createCollisionShape(pybullet.GEOM_MESH, fileName=filename, meshScale=scale, - **kwargs) + # create collision shape if specified + collision_shape = None + if with_collision: + collision_shape = self.sim.createCollisionShape(pybullet.GEOM_MESH, fileName=filename, meshScale=scale, + **kwargs) if color is not None: kwargs['rgbaColor'] = color @@ -586,11 +600,17 @@ class Bullet(Simulator): visual_shape = self.sim.createVisualShape(pybullet.GEOM_MESH, fileName=filename, meshScale=scale, **kwargs) # create body - mesh = self.sim.createMultiBody(baseMass=mass, - baseCollisionShapeIndex=collision_shape, - baseVisualShapeIndex=visual_shape, - basePosition=position, - baseOrientation=orientation) + if with_collision: + mesh = self.sim.createMultiBody(baseMass=mass, + baseCollisionShapeIndex=collision_shape, + baseVisualShapeIndex=visual_shape, + basePosition=position, + baseOrientation=orientation) + else: + mesh = self.sim.createMultiBody(baseMass=mass, + baseVisualShapeIndex=visual_shape, + basePosition=position, + baseOrientation=orientation) return mesh @@ -599,15 +619,15 @@ class Bullet(Simulator): ########## # TODO: add the other arguments - def create_body(self, visual_shape_id=-1, collision_shape_id=-1, mass=0, position=(0., 0., 0.), - orientation=(0., 0., 0., 1.)): + def create_body(self, visual_shape_id=-1, collision_shape_id=-1, mass=0., position=(0., 0., 0.), + orientation=(0., 0., 0., 1.), *args, **kwargs): """Create a body in the simulator. Args: visual_shape_id (int): unique id from createVisualShape or -1. You can reuse the visual shape (instancing) collision_shape_id (int): unique id from createCollisionShape or -1. You can re-use the collision shape for multiple multibodies (instancing) - mass (int): mass of the base, in kg (if using SI units) + mass (float): mass of the base, in kg (if using SI units) position (np.float[3]): Cartesian world position of the base orientation (np.float[4]): Orientation of base as quaternion [x,y,z,w] @@ -618,11 +638,9 @@ class Bullet(Simulator): position = position.ravel().tolist() if isinstance(orientation, np.ndarray): orientation = orientation.ravel().tolist() - elif isinstance(orientation, quaternion.quaternion): - orientation = self.quat_conv.convertFrom(orientation) return self.sim.createMultiBody(baseMass=mass, baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id, basePosition=position, - baseOrientation=orientation) + baseOrientation=orientation, **kwargs) def remove_body(self, body_id): """Remove a particular body in the simulator. @@ -848,7 +866,7 @@ class Bullet(Simulator): """ return self.sim.getBodyInfo(body_id)[0] - def get_center_of_mass(self, body_id, link_ids=None): + def get_center_of_mass_position(self, body_id, link_ids=None): """ Return the center of mass position. @@ -867,9 +885,42 @@ class Bullet(Simulator): mass = self.get_link_masses(body_id, link_ids) com = np.sum(pos.T * mass, axis=1) / np.sum(mass) - return com + def get_center_of_mass_velocity(self, body_id, link_ids=None): + """ + Return the center of mass linear velocity. + + Args: + body_id (int): unique body id. + link_ids (list of int): link ids associated with the given body id. If None, it will take all the links + of the specified body. + + Returns: + np.float[3]: center of mass linear velocity. + """ + if link_ids is None: + link_ids = list(range(self.num_links(body_id))) + + vel = self.get_link_world_linear_velocities(body_id, link_ids) + mass = self.get_link_masses(body_id, link_ids) + + com = np.sum(vel.T * mass, axis=1) / np.sum(mass) + return com + + def get_linear_momentum(self, body_id, link_ids=None): + """ + Return the total linear momentum in the world space. + + Returns: + np.float[3]: linear momentum + """ + if link_ids is None: + link_ids = list(range(self.num_links(body_id))) + mass = self.get_link_masses(body_id, link_ids) + vel = self.get_link_world_linear_velocities(body_id, link_ids) + return np.sum(vel.T * mass, axis=1) + def get_base_pose(self, body_id): """ Get the current position and orientation of the base (or root link) of the body in Cartesian world coordinates. @@ -1026,7 +1077,7 @@ class Bullet(Simulator): "This method will only work when explicitly stepping the simulation using stepSimulation, in other words: setRealTimeSimulation(0). After each simulation step, the external forces are cleared to zero. If you are - using 'setRealTimeSimulation(1), applyExternalForce/Torque will have undefined behavior (either 0, 1 or + using 'setRealTimeSimulation(1), apply_external_force/Torque will have undefined behavior (either 0, 1 or multiple force/torque applications)" [1] Args: @@ -1074,6 +1125,21 @@ class Bullet(Simulator): """ return self.sim.getNumJoints(body_id) + def num_actuated_joints(self, body_id): + """ + Return the total number of actuated joints associated with the given body id. + + Warnings: this checks through the list of all joints each time it is called. It might be a good idea to call + this method one time and cache the actuated joint ids. + + Args: + body_id (int): unique body id. + + Returns: + int: number of actuated joints of the specified body. + """ + return len(self.get_actuated_joint_ids(body_id)) + def num_links(self, body_id): """ Return the total number of links of the specified body. This is the same as calling `num_joints`. @@ -1120,7 +1186,7 @@ class Bullet(Simulator): [15] np.float[4]: joint orientation in parent frame [16] int: parent link index, -1 for base """ - info = self.sim.getJointInfo(body_id, joint_id) + info = list(self.sim.getJointInfo(body_id, joint_id)) info[13] = np.array(info[13]) info[14] = np.array(info[14]) info[15] = np.array(info[15]) @@ -1164,9 +1230,12 @@ class Bullet(Simulator): VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the applied joint motor torque is exactly what you provide, so there is no need to report it separately. """ - return self.sim.getJointStates(body_id, joint_ids) + states = self.sim.getJointStates(body_id, joint_ids) + for idx, state in enumerate(states): + states[idx][2] = np.array(state[2]) + return states - def reset_joint_state(self, body_id, joint_id, target_position, target_velocity=0.): + def reset_joint_state(self, body_id, joint_id, position, velocity=0.): """ Reset the state of the joint. It is best only to do this at the start, while not running the simulation: `reset_joint_state` overrides all physics simulation. Note that we only support 1-DOF motorized joints at @@ -1175,12 +1244,12 @@ class Bullet(Simulator): Args: body_id (int): body unique id as returned by `load_urdf`, etc. joint_id (int): joint index in range [0..num_joints(body_id)] - target_position (float): the joint position (angle in radians [rad] or position [m]) - target_velocity (float): the joint velocity (angular [rad/s] or linear velocity [m/s]) + position (float): the joint position (angle in radians [rad] or position [m]) + velocity (float): the joint velocity (angular [rad/s] or linear velocity [m/s]) """ - self.sim.resetJointState(body_id, joint_id, target_position, target_velocity) + self.sim.resetJointState(body_id, joint_id, position, velocity) - def enable_joint_force_torque_sensor(self, body_id, joint_id, enable=True): + def enable_joint_force_torque_sensor(self, body_id, joint_ids, enable=True): """ You can enable or disable a joint force/torque sensor in each joint. Once enabled, if you perform a `step`, the 'get_joint_state' will report the joint reaction forces in the fixed degrees of freedom: a fixed @@ -1190,13 +1259,17 @@ class Bullet(Simulator): Args: body_id (int): body unique id as returned by `load_urdf`, etc. - joint_id (int): joint index in range [0..num_joints(body_id)] + joint_ids (int, int[N]): joint index in range [0..num_joints(body_id)], or list of joint ids. enable (bool): True to enable, False to disable the force/torque sensor """ - self.sim.enableJointForceTorqueSensor(body_id, joint_id, enable) + if isinstance(joint_ids, int): + self.sim.enableJointForceTorqueSensor(body_id, joint_ids, int(enable)) + else: + for joint_id in joint_ids: + self.sim.enableJointForceTorqueSensor(body_id, joint_id, int(enable)) - def set_joint_motor_control(self, body_id, joint_id, control_mode=pybullet.POSITION_CONTROL, position=None, - velocity=None, force=None, kp=None, kd=None, max_velocity=None): + def set_joint_motor_control(self, body_id, joint_ids, control_mode=pybullet.POSITION_CONTROL, positions=None, + velocities=None, forces=None, kp=None, kd=None, max_velocity=None): """ Set the joint motor control. @@ -1219,69 +1292,49 @@ class Bullet(Simulator): Args: body_id (int): body unique id. - joint_id (int): joint/link id. + joint_ids (int): joint/link id, or list of joint ids. control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD), VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3). - position (float): target joint position (used in POSITION_CONTROL). - velocity (float): target joint velocity. In VELOCITY_CONTROL and POSITION_CONTROL, the target velocity is - the desired velocity of the joint. Note that the target velocity is not the maximum joint velocity. - In PD_CONTROL and POSITION_CONTROL/CONTROL_MODE_POSITION_VELOCITY_PD, the final target velocity is - computed using: + positions (float, np.float[N]): target joint position(s) (used in POSITION_CONTROL). + velocities (float, np.float[N]): target joint velocity(ies). In VELOCITY_CONTROL and POSITION_CONTROL, + the target velocity(ies) is(are) the desired velocity of the joint. Note that the target velocity(ies) + is(are) not the maximum joint velocity(ies). In PD_CONTROL and + POSITION_CONTROL/CONTROL_MODE_POSITION_VELOCITY_PD, the final target velocities are computed using: `kp*(erp*(desiredPosition-currentPosition)/dt)+currentVelocity+kd*(m_desiredVelocity - currentVelocity)` - force (float): in POSITION_CONTROL and VELOCITY_CONTROL, this is the maximum motor force used to reach the - target value. In TORQUE_CONTROL this is the force/torque to be applied each simulation step. - kp (float): position (stiffness) gain (used in POSITION_CONTROL). - kd (float): velocity (damping) gain (used in POSITION_CONTROL). + forces (float, list of float): in POSITION_CONTROL and VELOCITY_CONTROL, these are the maximum motor + forces used to reach the target values. In TORQUE_CONTROL these are the forces / torques to be applied + each simulation step. + kp (float, list of float): position (stiffness) gain(s) (used in POSITION_CONTROL). + kd (float, list of float): velocity (damping) gain(s) (used in POSITION_CONTROL). max_velocity (float): in POSITION_CONTROL this limits the velocity to a maximum. """ kwargs = {} - if position is not None: - kwargs['targetPosition'] = position - if velocity is not None: - kwargs['targetVelocity'] = velocity - if force is not None: - kwargs['force'] = force - if kp is not None: - kwargs['positionGain'] = kp - if kd is not None: - kwargs['velocityGain'] = kd - if max_velocity is not None: - kwargs['maxVelocity'] = max_velocity - self.sim.setJointMotorControl2(body_id, joint_id, controlMode=control_mode, **kwargs) - - def set_joint_motor_control_array(self, body_id, joint_ids, control_mode=pybullet.POSITION_CONTROL, positions=None, - velocities=None, forces=None, kps=None, kds=None): - """ - Instead of making individual calls for each joint, you can pass arrays for all inputs to reduce calling - overhead dramatically. - - Args: - body_id (int): body unique id. - joint_ids (list of int): list of joint id. - control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD), - VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3). - positions (list of float): list of target joint positions (used in POSITION_CONTROL) the target value is - target position of the joint. - velocities (list of float): list of target joint velocities (used in PD_CONTROL, VELOCITY_CONTROL and - POSITION_CONTROL). - forces (list of float): list of forces. In POSITION_CONTROL and VELOCITY_CONTROL, these are the maximum - motor forces used to reach the target values. In TORQUE_CONTROL these are the forces/torques to be - applied each simulation step. - kps (list of float): list of position (stiffness) gains (used in POSITION_CONTROL). - kds (list of float): list of velocity (damping) gains (used in POSITION_CONTROL). - """ - kwargs = {} - if positions is not None: - kwargs['targetPositions'] = positions - if velocities is not None: - kwargs['targetVelocities'] = velocities - if forces is not None: - kwargs['forces'] = forces - if kps is not None: - kwargs['positionGains'] = kps - if kds is not None: - kwargs['velocityGains'] = kds - self.sim.setJointMotorControlArray(body_id, joint_ids, controlMode=control_mode, **kwargs) + if isinstance(joint_ids, int): + if positions is not None: + kwargs['targetPosition'] = positions + if velocities is not None: + kwargs['targetVelocity'] = velocities + if forces is not None: + kwargs['force'] = forces + if kp is not None: + kwargs['positionGain'] = kp + if kd is not None: + kwargs['velocityGain'] = kd + if max_velocity is not None: + kwargs['maxVelocity'] = max_velocity + self.sim.setJointMotorControl2(body_id, joint_ids, controlMode=control_mode, **kwargs) + else: + if positions is not None: + kwargs['targetPositions'] = positions + if velocities is not None: + kwargs['targetVelocities'] = velocities + if forces is not None: + kwargs['forces'] = forces + if kp is not None: + kwargs['positionGains'] = kp + if kd is not None: + kwargs['velocityGains'] = kd + self.sim.setJointMotorControlArray(body_id, joint_ids, controlMode=control_mode, **kwargs) def get_link_state(self, body_id, link_id, compute_velocity=False, compute_forward_kinematics=False): """ @@ -1305,8 +1358,8 @@ class Bullet(Simulator): np.float[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True. np.float[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True. """ - results = self.sim.getLinkState(body_id, link_id, computeLinkVelocity=compute_velocity, - computeForwardKinematics=compute_forward_kinematics) + results = self.sim.getLinkState(body_id, link_id, computeLinkVelocity=int(compute_velocity), + computeForwardKinematics=int(compute_forward_kinematics)) return [np.array(result) for result in results] def get_link_states(self, body_id, link_ids, compute_velocity=False, compute_forward_kinematics=False): @@ -1517,7 +1570,7 @@ class Bullet(Simulator): def get_link_velocities(self, body_id, link_ids): pass - def get_qindex(self, body_id, joint_ids): + def get_q_indices(self, body_id, joint_ids): """ Get the corresponding q index of the given joint(s). @@ -1551,9 +1604,9 @@ class Bullet(Simulator): joint_ids = [] for joint_id in range(self.num_joints(body_id)): # Get joint info - jnt = self.get_joint_info(body_id, joint_id) - if jnt[2] != self.sim.JOINT_FIXED: # if not a fixed joint - joint_ids.append(jnt[0]) + joint = self.get_joint_info(body_id, joint_id) + if joint[2] != self.sim.JOINT_FIXED: # if not a fixed joint + joint_ids.append(joint[0]) return joint_ids def get_joint_names(self, body_id, joint_ids): @@ -1574,6 +1627,41 @@ class Bullet(Simulator): return self.sim.getJointInfo(body_id, joint_ids)[1] return [self.sim.getJointInfo(body_id, joint_id)[1] for joint_id in joint_ids] + def get_joint_type_ids(self, body_id, joint_ids): + """ + Get the joint type ids. + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + int: joint type id. + if multiple joints: list of above + """ + if isinstance(joint_ids, int): + return self.sim.getJointInfo(body_id, joint_ids)[2] + return [self.sim.getJointInfo(body_id, joint_id)[2] for joint_id in joint_ids] + + def get_joint_type_names(self, body_id, joint_ids): + """ + Get joint type names. + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + str: joint type name. + if multiple joints: list of above + """ + joint_type_names = ['revolute', 'prismatic', 'spherical', 'planar', 'fixed', 'point2point', 'gear'] + if isinstance(joint_ids, int): + return joint_type_names[self.sim.getJointInfo(body_id, joint_ids)[2]] + return [joint_type_names[self.sim.getJointInfo(body_id, joint_id)[2]] for joint_id in joint_ids] + def get_joint_dampings(self, body_id, joint_ids): """ Get the damping coefficient of the given joint(s). @@ -1604,7 +1692,7 @@ class Bullet(Simulator): if 1 joint: float: friction coefficient of the given joint if multiple joints: - float[N]: friction coefficient for each specified joint + np.float[N]: friction coefficient for each specified joint """ if isinstance(joint_ids, int): return self.sim.getJointInfo(body_id, joint_ids)[7] @@ -1642,7 +1730,7 @@ class Bullet(Simulator): if 1 joint: float: maximum force [N] if multiple joints: - float[N]: maximum force for each specified joint [N] + np.float[N]: maximum force for each specified joint [N] """ if isinstance(joint_ids, int): return self.sim.getJointInfo(body_id, joint_ids)[10] @@ -1699,13 +1787,8 @@ class Bullet(Simulator): kds (None, float, np.float[N]): velocity gain(s) forces (None, float, np.float[N]): maximum motor force(s)/torque(s) used to reach the target values. """ - if isinstance(joint_ids, int): - self.set_joint_motor_control(body_id, joint_ids, control_mode=pybullet.POSITION_CONTROL, position=positions, - velocity=velocities, force=forces, kp=kps, kd=kds) - else: - self.set_joint_motor_control_array(body_id, joint_ids, control_mode=pybullet.POSITION_CONTROL, - positions=positions, velocities=velocities, forces=forces, kps=kps, - kds=kds) + self.set_joint_motor_control(body_id, joint_ids, control_mode=pybullet.POSITION_CONTROL, positions=positions, + velocities=velocities, forces=forces, kp=kps, kd=kds) def get_joint_positions(self, body_id, joint_ids): """ @@ -1796,7 +1879,7 @@ class Bullet(Simulator): # if joint accelerations vector is not the same size as the actuated joints if len(accelerations) != num_actuated_joints: - q_idx = self.get_qindex(joint_ids) + q_idx = self.get_q_indices(joint_ids) acc = np.zeros(num_actuated_joints) acc[q_idx] = accelerations accelerations = acc @@ -1806,7 +1889,7 @@ class Bullet(Simulator): # get corresponding torques if len(torques) != len(joint_ids): - q_idx = self.get_qindex(joint_ids) + q_idx = self.get_q_indices(joint_ids) torques = torques[q_idx] # set the joint torques @@ -1844,7 +1927,7 @@ class Bullet(Simulator): accelerations = self.calculate_forward_dynamics(body_id, q, dq, torques=torques) # return the specified accelerations - q_idx = self.get_qindex(body_id, joint_ids) + q_idx = self.get_q_indices(body_id, joint_ids) return accelerations[q_idx] def set_joint_torques(self, body_id, joint_ids, torques): @@ -1992,26 +2075,33 @@ class Bullet(Simulator): def get_visual_shape_data(self, object_id, flags=-1): """ - Get the visual shape data associated with the given object id. + Get the visual shape data associated with the given object id. It will output a list of visual shape data. Args: object_id (int): object unique id. flags (int, None): VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) will also provide `texture_unique_id`. Returns: - int: object unique id. - int: link index or -1 for the base - int: visual geometry type (TBD) - np.float[3]: dimensions (size, local scale) of the geometry - str: path to the triangle mesh, if any. Typically relative to the URDF, SDF or MJCF file location, but - could be absolute - np.float[3]: position of local visual frame, relative to link/joint frame - np.float[4]: orientation of local visual frame relative to link/joint frame - list of 4 floats: URDF color (if any specified) in Red / Green / Blue / Alpha - int: texture unique id of the shape or -1 if None. This field only exists if using - VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) flag. + list: + int: object unique id. + int: link index or -1 for the base + int: visual geometry type (TBD) + np.float[3]: dimensions (size, local scale) of the geometry + str: path to the triangle mesh, if any. Typically relative to the URDF, SDF or MJCF file location, but + could be absolute + np.float[3]: position of local visual frame, relative to link/joint frame + np.float[4]: orientation of local visual frame relative to link/joint frame + list of 4 floats: URDF color (if any specified) in Red / Green / Blue / Alpha + int: texture unique id of the shape or -1 if None. This field only exists if using + VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) flag. """ - return self.sim.getVisualShapeData(object_id, flags=flags) + shapes = list(self.sim.getVisualShapeData(object_id, flags=flags)) + for idx, shape in enumerate(shapes): + shapes[idx] = list(shape) + shapes[idx][3] = np.array(shape[3]) + shapes[idx][5] = np.array(shape[5]) + shapes[idx][6] = np.array(shape[6]) + return shapes def change_visual_shape(self, object_id, link_id, shape_id=None, texture_id=None, rgba_color=None, specular_color=None): @@ -2071,7 +2161,8 @@ class Bullet(Simulator): [1] http://www.codinglabs.net/article_world_view_projection_matrix.aspx [2] http://www.thecodecrate.com/opengl-es/opengl-transformation-matrices/ """ - view = self.sim.computeViewMatrix(eyePosition=eye_position, targetPosition=target_position, upVector=up_vector) + view = self.sim.computeViewMatrix(cameraEyePosition=eye_position, cameraTargetPosition=target_position, + cameraUpVector=up_vector) return np.array(view).reshape(4, 4).T def compute_view_matrix_from_ypr(self, target_position, distance, yaw, pitch, roll, up_axis_index=2): @@ -2096,7 +2187,7 @@ class Bullet(Simulator): [1] http://www.codinglabs.net/article_world_view_projection_matrix.aspx [2] http://www.thecodecrate.com/opengl-es/opengl-transformation-matrices/ """ - view = self.sim.computeViewMatrixFromYawPitchRoll(targetPosition=target_position, distance=distance, + view = self.sim.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=target_position, distance=distance, yaw=np.rad2deg(yaw), pitch=np.rad2deg(pitch), roll=np.rad2deg(roll), upAxisIndex=up_axis_index) return np.array(view).reshape(4, 4).T @@ -2189,7 +2280,7 @@ class Bullet(Simulator): z-buffer. See https://stackoverflow.com/questions/6652253/getting-the-true-z-value-from-the-depth-buffer Using the projection matrix, the depth is computed as: `depth = far * near / (far - (far - near) * depthImg)`, where `depthImg` is the depth from Bullet - `getCameraImage`, far=1000. and near=0.01. + `get_camera_image`, far=1000. and near=0.01. np.int[width, height]: Segmentation mask buffer. For each pixels the visible object unique id. If ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX (=1) is used, the segmentationMaskBuffer combines the object unique id and link index as follows: value = objectUniqueId + (linkIndex+1)<<24. @@ -2346,7 +2437,7 @@ class Bullet(Simulator): z-buffer. See https://stackoverflow.com/questions/6652253/getting-the-true-z-value-from-the-depth-buffer Using the projection matrix, the depth is computed as: `depth = far * near / (far - (far - near) * depthImg)`, where `depthImg` is the depth from Bullet - `getCameraImage`, far=1000. and near=0.01. + `get_camera_image`, far=1000. and near=0.01. """ kwargs = {} if view_matrix is not None: @@ -2572,16 +2663,16 @@ class Bullet(Simulator): aabb_min, aabb_max = self.sim.getAABB(body_id, link_id) return np.array(aabb_min), np.array(aabb_max) - def get_contact_points(self, body_a, body_b, link_id_a=None, link_id_b=None): + def get_contact_points(self, body1, body2, link1_id=None, link2_id=None): """ Returns the contact points computed during the most recent call to `step`. Args: - body_a (int): only report contact points that involve body A - body_b (int): only report contact points that involve body B. Important: you need to have a valid body A + body1 (int): only report contact points that involve body A + body2 (int): only report contact points that involve body B. Important: you need to have a valid body A if you provide body B - link_id_a (int): only report contact points that involve link index of body A - link_id_b (int): only report contact points that involve link index of body B + link1_id (int): only report contact points that involve link index of body A + link2_id (int): only report contact points that involve link index of body B Returns: list: @@ -2601,14 +2692,14 @@ class Bullet(Simulator): np.float[3]: second lateral friction direction """ kwargs = {} - if body_a is not None: - kwargs['bodyA'] = body_a - if link_id_a is not None: - kwargs['linkIndexA'] = link_id_a - if body_b is not None: - kwargs['bodyB'] = body_b - if link_id_b is not None: - kwargs['linkIndexB'] = link_id_b + if body1 is not None: + kwargs['bodyA'] = body1 + if link1_id is not None: + kwargs['linkIndexA'] = link1_id + if body2 is not None: + kwargs['bodyB'] = body2 + if link2_id is not None: + kwargs['linkIndexB'] = link2_id results = self.sim.getContactPoints(**kwargs) if len(results) == 0: @@ -2616,18 +2707,18 @@ class Bullet(Simulator): return [[r[0], r[1], r[2], r[3], r[4], np.array(r[5]), np.array(r[6]), np.array(r[7]), r[8], r[9], r[10], np.array(r[11]), r[12], np.array(r[13])] for r in results] - def get_closest_points(self, body_a, body_b, distance, link_id_a=None, link_id_b=None): + def get_closest_points(self, body1, body2, distance, link1_id=None, link2_id=None): """ Computes the closest points, independent from `step`. This also lets you compute closest points of objects with an arbitrary separating distance. In this query there will be no normal forces reported. Args: - body_a (int): only report contact points that involve body A - body_b (int): only report contact points that involve body B. Important: you need to have a valid body A + body1 (int): only report contact points that involve body A + body2 (int): only report contact points that involve body B. Important: you need to have a valid body A if you provide body B distance (float): If the distance between objects exceeds this maximum distance, no points may be returned. - link_id_a (int): only report contact points that involve link index of body A - link_id_b (int): only report contact points that involve link index of body B + link1_id (int): only report contact points that involve link index of body A + link2_id (int): only report contact points that involve link index of body B Returns: list: @@ -2647,12 +2738,12 @@ class Bullet(Simulator): np.float[3]: second lateral friction direction """ kwargs = {} - if link_id_a is not None: - kwargs['linkIndexA'] = link_id_a - if link_id_b is not None: - kwargs['linkIndexB'] = link_id_b + if link1_id is not None: + kwargs['linkIndexA'] = link1_id + if link2_id is not None: + kwargs['linkIndexB'] = link2_id - results = self.sim.getContactPoints(body_a, body_b, distance, **kwargs) + results = self.sim.getContactPoints(body1, body2, distance, **kwargs) if len(results) == 0: return results return [[r[0], r[1], r[2], r[3], r[4], np.array(r[5]), np.array(r[6]), np.array(r[7]), r[8], r[9], r[10], @@ -2667,18 +2758,19 @@ class Bullet(Simulator): to_position (np.float[3]): end of the ray in world coordinates Returns: - int: object unique id of the hit object - int: link index of the hit object, or -1 if none/parent - float: hit fraction along the ray in range [0,1] along the ray. - np.float[3]: hit position in Cartesian world coordinates - np.float[3]: hit normal in Cartesian world coordinates + list: + int: object unique id of the hit object + int: link index of the hit object, or -1 if none/parent + float: hit fraction along the ray in range [0,1] along the ray. + np.float[3]: hit position in Cartesian world coordinates + np.float[3]: hit normal in Cartesian world coordinates """ if isinstance(from_position, np.ndarray): from_position = from_position.ravel().tolist() if isinstance(to_position, np.ndarray): to_position = to_position.ravel().tolist() - object_id, link_id, hit_fraction, position, normal = self.sim.rayTest(from_position, to_position) - return object_id, link_id, hit_fraction, np.array(position), np.array(normal) + collisions = self.sim.rayTest(from_position, to_position) + return [[c[0], c[1], c[2], np.array(c[3]), np.array(c[4])] for c in collisions] def ray_test_batch(self, from_positions, to_positions, parent_object_id=None, parent_link_id=None): """Perform a batch of raycasts to find the intersection information of the first objects hit. @@ -2733,18 +2825,18 @@ class Bullet(Simulator): """ self.sim.setCollisionFilterGroupMask(body_id, link_id, filter_group, filter_mask) - def set_collision_filter_pair(self, body_a, body_b, link_a=-1, link_b=-1, enable=True): + def set_collision_filter_pair(self, body1, body2, link1=-1, link2=-1, enable=True): """ Enable/disable collision between two bodies/links. Args: - body_a (int): unique id of body A to be filtered - body_b (int): unique id of body B to be filtered, A==B implies self-collision - link_a (int): link index of body A - link_b (int): link index of body B + body1 (int): unique id of body A to be filtered + body2 (int): unique id of body B to be filtered, A==B implies self-collision + link1 (int): link index of body A + link2 (int): link index of body B enable (bool): True to enable collision, False to disable collision """ - self.sim.setCollisionFilterPair(body_a, body_b, link_a, link_b, int(enable)) + self.sim.setCollisionFilterPair(body1, body2, link1, link2, int(enable)) ########################### # Kinematics and Dynamics # @@ -2771,8 +2863,10 @@ class Bullet(Simulator): float: damping of contact constraints. -1 if not available. float: stiffness of contact constraints. -1 if not available. """ - mass, friction, inertia, pos, quat, restitution, roll, spin, kd, kp = self.sim.getDynamicsInfo(body_id, link_id) - return mass, friction, np.array(inertia), np.array(pos), np.array(quat), restitution, roll, spin, kd, kp + info = list(self.sim.getDynamicsInfo(body_id, link_id)) + for i in range(2, 5): + info[i] = np.array(info[i]) + return info def change_dynamics(self, body_id, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None, rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None, @@ -2894,7 +2988,7 @@ class Bullet(Simulator): np.float[N,N], np.float[6+N,6+N]: inertia matrix """ if isinstance(q, np.ndarray): - q = q.ravel().tolist() + q = q.ravel().tolist() # Note that pybullet doesn't accept numpy arrays here return np.array(self.sim.calculateMassMatrix(body_id, q)) def calculate_inverse_kinematics(self, body_id, link_id, position, orientation=None, lower_limits=None, @@ -2927,7 +3021,7 @@ class Bullet(Simulator): solver (int): p.IK_DLS (=0) or p.IK_SDLS (=1), Damped Least Squares or Selective Damped Least Squares, as described in the paper by Samuel Buss "Selectively Damped Least Squares for Inverse Kinematics". q_curr (np.float[N]): list of joint positions. By default PyBullet uses the joint positions of the body. - If provided, the targetPosition and targetOrientation is in local space! + If provided, the target_position and targetOrientation is in local space! max_iters (int): maximum number of iterations. Refine the IK solution until the distance between target and actual end effector position is below this threshold, or the `max_iters` is reached. threshold (float): residual threshold. Refine the IK solution until the distance between target and actual @@ -3058,7 +3152,7 @@ class Bullet(Simulator): torques (np.float[N]): desired joint torques Returns: - float[N]: joint accelerations computed using the rigid-body equation of motion + np.float[N]: joint accelerations computed using the rigid-body equation of motion References: [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1 diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index 9fe03ea..1297e8f 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -48,6 +48,142 @@ class Simulator(object): self._render = render self.real_time = False + # TODO: this is really bad to have attributes like that... It doesn't generalize well to other simulators... + + # import pybullet + # for attribute in dir(pybullet): + # if attribute[0].isupper(): + # print('self.{} = {}'.format(attribute, getattr(pybullet, attribute))) + + self.B3G_ALT = 65308 + self.B3G_BACKSPACE = 65305 + self.B3G_CONTROL = 65307 + self.B3G_DELETE = 65304 + self.B3G_DOWN_ARROW = 65298 + self.B3G_END = 65301 + self.B3G_F1 = 65280 + self.B3G_F10 = 65289 + self.B3G_F11 = 65290 + self.B3G_F12 = 65291 + self.B3G_F13 = 65292 + self.B3G_F14 = 65293 + self.B3G_F15 = 65294 + self.B3G_F2 = 65281 + self.B3G_F3 = 65282 + self.B3G_F4 = 65283 + self.B3G_F5 = 65284 + self.B3G_F6 = 65285 + self.B3G_F7 = 65286 + self.B3G_F8 = 65287 + self.B3G_F9 = 65288 + self.B3G_HOME = 65302 + self.B3G_INSERT = 65303 + self.B3G_LEFT_ARROW = 65295 + self.B3G_PAGE_DOWN = 65300 + self.B3G_PAGE_UP = 65299 + self.B3G_RETURN = 65309 + self.B3G_RIGHT_ARROW = 65296 + self.B3G_SHIFT = 65306 + self.B3G_UP_ARROW = 65297 + + self.COV_ENABLE_DEPTH_BUFFER_PREVIEW = 14 + self.COV_ENABLE_GUI = 1 + self.COV_ENABLE_KEYBOARD_SHORTCUTS = 9 + self.COV_ENABLE_MOUSE_PICKING = 10 + self.COV_ENABLE_PLANAR_REFLECTION = 16 + self.COV_ENABLE_RENDERING = 7 + self.COV_ENABLE_RGB_BUFFER_PREVIEW = 13 + self.COV_ENABLE_SEGMENTATION_MARK_PREVIEW = 15 + self.COV_ENABLE_SHADOWS = 2 + self.COV_ENABLE_SINGLE_STEP_RENDERING = 17 + self.COV_ENABLE_TINY_RENDERER = 12 + self.COV_ENABLE_WIREFRAME = 3 + self.COV_ENABLE_Y_AXIS_UP = 11 + + self.DIRECT = 2 + self.ER_BULLET_HARDWARE_OPENGL = 131072 + self.ER_NO_SEGMENTATION_MASK = 4 + self.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX = 1 + self.ER_TINY_RENDERER = 65536 + self.ER_USE_PROJECTIVE_TEXTURE = 2 + + self.GEOM_FORCE_CONCAVE_TRIMESH = 1 + self.GEOM_SPHERE = 2 + self.GEOM_CONCAVE_INTERNAL_EDGE = 2 + self.GEOM_BOX = 3 + self.GEOM_CYLINDER = 4 + self.GEOM_MESH = 5 + self.GEOM_PLANE = 6 + self.GEOM_CAPSULE = 7 + + self.GUI = 1 + self.GUI_MAIN_THREAD = 8 + self.GUI_SERVER = 7 + self.IK_DLS = 0 + self.IK_HAS_JOINT_DAMPING = 128 + self.IK_HAS_NULL_SPACE_VELOCITY = 64 + self.IK_HAS_TARGET_ORIENTATION = 32 + self.IK_HAS_TARGET_POSITION = 16 + self.IK_SDLS = 1 + + self.JOINT_FEEDBACK_IN_JOINT_FRAME = 2 + self.JOINT_FEEDBACK_IN_WORLD_SPACE = 1 + self.JOINT_FIXED = 4 + self.JOINT_GEAR = 6 + self.JOINT_PLANAR = 3 + self.JOINT_POINT2POINT = 5 + self.JOINT_PRISMATIC = 1 + self.JOINT_REVOLUTE = 0 + self.JOINT_SPHERICAL = 2 + + self.KEY_IS_DOWN = 1 + self.KEY_WAS_RELEASED = 4 + self.KEY_WAS_TRIGGERED = 2 + + self.LINK_FRAME = 1 + self.WORLD_FRAME = 2 + + self.MAX_RAY_INTERSECTION_BATCH_SIZE = 16384 + + self.VELOCITY_CONTROL = 0 + self.TORQUE_CONTROL = 1 + self.POSITION_CONTROL = 2 + self.PD_CONTROL = 3 + + self.SENSOR_FORCE_TORQUE = 1 + self.SHARED_MEMORY = 3 + self.SHARED_MEMORY_KEY = 12347 + self.SHARED_MEMORY_KEY2 = 12348 + self.SHARED_MEMORY_SERVER = 9 + self.STATE_LOGGING_ALL_COMMANDS = 7 + self.STATE_LOGGING_CONTACT_POINTS = 5 + self.STATE_LOGGING_CUSTOM_TIMER = 9 + self.STATE_LOGGING_GENERIC_ROBOT = 1 + self.STATE_LOGGING_MINITAUR = 0 + self.STATE_LOGGING_PROFILE_TIMINGS = 6 + self.STATE_LOGGING_VIDEO_MP4 = 3 + self.STATE_LOGGING_VR_CONTROLLERS = 2 + self.STATE_LOG_JOINT_MOTOR_TORQUES = 1 + self.STATE_LOG_JOINT_TORQUES = 3 + self.STATE_LOG_JOINT_USER_TORQUES = 2 + self.STATE_REPLAY_ALL_COMMANDS = 8 + + self.TCP = 5 + self.UDP = 4 + + self.URDF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024 + self.URDF_ENABLE_SLEEPING = 2048 + self.URDF_GLOBAL_VELOCITIES_MB = 256 + self.URDF_INITIALIZE_SAT_FEATURES = 4096 + self.URDF_USE_IMPLICIT_CYLINDER = 128 + self.URDF_USE_INERTIA_FROM_FILE = 2 + self.URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768 + self.URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536 + self.URDF_USE_SELF_COLLISION = 8 + self.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32 + self.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16 + self.URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192 + ############## # Properties # ############## @@ -249,7 +385,7 @@ class Simulator(object): pass def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None, - flags=None, *args, **kwargs): + with_collision=True, flags=None, *args, **kwargs): """Load a mesh into the simulator. Args: @@ -261,6 +397,7 @@ class Simulator(object): mass (float): mass of the mesh (in kg). If mass = 0, it won't move even if there is a collision. scale (float[3]): scale the mesh in the (x,y,z) directions color (int[4], None): color of the mesh (by default: white and opaque) + with_collision (bool): If True, it will also create the collision mesh, and not only a visual mesh. flags (int, None): if flag = `sim.GEOM_FORCE_CONCAVE_TRIMESH` (=1), this will create a concave static triangle mesh. This should not be used with dynamic/moving objects, only for static (mass=0) terrain. @@ -271,15 +408,15 @@ class Simulator(object): # bodies - def create_body(self, visual_shape_id=-1, collision_shape_id=-1, mass=0, position=(0., 0., 0.), - orientation=(0., 0., 0., 1.)): + def create_body(self, visual_shape_id=-1, collision_shape_id=-1, mass=0., position=(0., 0., 0.), + orientation=(0., 0., 0., 1.), *args, **kwargs): """Create a body in the simulator. Args: visual_shape_id (int): unique id from createVisualShape or -1. You can reuse the visual shape (instancing) collision_shape_id (int): unique id from createCollisionShape or -1. You can re-use the collision shape for multiple multibodies (instancing) - mass (int): mass of the base, in kg (if using SI units) + mass (float): mass of the base, in kg (if using SI units) position (np.float[3]): Cartesian world position of the base orientation (np.float[4]): Orientation of base as quaternion [x,y,z,w] @@ -454,7 +591,7 @@ class Simulator(object): """ pass - def get_center_of_mass(self, body_id, link_ids=None): + def get_center_of_mass_position(self, body_id, link_ids=None): """ Return the center of mass position. @@ -468,6 +605,20 @@ class Simulator(object): """ pass + def get_center_of_mass_velocity(self, body_id, link_ids=None): + """ + Return the center of mass linear velocity. + + Args: + body_id (int): unique body id. + link_ids (list of int): link ids associated with the given body id. If None, it will take all the links + of the specified body. + + Returns: + np.float[3]: center of mass linear velocity. + """ + pass + def get_base_pose(self, body_id): """ Get the current position and orientation of the base (or root link) of the body in Cartesian world coordinates. @@ -645,6 +796,18 @@ class Simulator(object): """ pass + def num_actuated_joints(self, body_id): + """ + Return the total number of actuated joints associated with the given body id. + + Args: + body_id (int): unique body id. + + Returns: + int: number of actuated joints of the specified body. + """ + pass + def num_links(self, body_id): """ Return the total number of links of the specified body. This is the same as calling `num_joints`. @@ -712,7 +875,7 @@ class Simulator(object): """ pass - def reset_joint_state(self, body_id, joint_id, target_position, target_velocity=0.): + def reset_joint_state(self, body_id, joint_id, position, velocity=0.): """ Reset the state of the joint. It is best only to do this at the start, while not running the simulation: `reset_joint_state` overrides all physics simulation. @@ -720,24 +883,24 @@ class Simulator(object): Args: body_id (int): unique body id. joint_id (int): joint index in range [0..num_joints(body_id)] - target_position (float): the joint position (angle in radians [rad] or position [m]) - target_velocity (float): the joint velocity (angular [rad/s] or linear velocity [m/s]) + position (float): the joint position (angle in radians [rad] or position [m]) + velocity (float): the joint velocity (angular [rad/s] or linear velocity [m/s]) """ pass - def enable_joint_force_torque_sensor(self, body_id, joint_id, enable=True): + def enable_joint_force_torque_sensor(self, body_id, joint_ids, enable=True): """ You can enable or disable a joint force/torque sensor in each joint. Args: body_id (int): body unique id. - joint_id (int): joint index in range [0..num_joints(body_id)] + joint_ids (int, int[N]): joint index in range [0..num_joints(body_id)], or list of joint ids. enable (bool): True to enable, False to disable the force/torque sensor """ pass - def set_joint_motor_control(self, body_id, joint_id, control_mode=2, position=None, - velocity=None, force=None, kp=None, kd=None, max_velocity=None): + def set_joint_motor_control(self, body_id, joint_ids, control_mode=2, positions=None, + velocities=None, forces=None, kp=None, kd=None, max_velocity=None): """ Set the joint motor control. @@ -751,46 +914,24 @@ class Simulator(object): Args: body_id (int): body unique id. - joint_id (int): joint/link id. + joint_ids (int): joint/link id, or list of joint ids. control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD), VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3). - position (float): target joint position (used in POSITION_CONTROL). - velocity (float): target joint velocity. In VELOCITY_CONTROL and POSITION_CONTROL, the target velocity is - the desired velocity of the joint. Note that the target velocity is not the maximum joint velocity. - In PD_CONTROL and POSITION_CONTROL/CONTROL_MODE_POSITION_VELOCITY_PD, the final target velocity is - computed using: + positions (float, np.float[N]): target joint position(s) (used in POSITION_CONTROL). + velocities (float, np.float[N]): target joint velocity(ies). In VELOCITY_CONTROL and POSITION_CONTROL, + the target velocity(ies) is(are) the desired velocity of the joint. Note that the target velocity(ies) + is(are) not the maximum joint velocity(ies). In PD_CONTROL and + POSITION_CONTROL/CONTROL_MODE_POSITION_VELOCITY_PD, the final target velocities are computed using: `kp*(erp*(desiredPosition-currentPosition)/dt)+currentVelocity+kd*(m_desiredVelocity - currentVelocity)` - force (float): in POSITION_CONTROL and VELOCITY_CONTROL, this is the maximum motor force used to reach the - target value. In TORQUE_CONTROL this is the force/torque to be applied each simulation step. - kp (float): position (stiffness) gain (used in POSITION_CONTROL). - kd (float): velocity (damping) gain (used in POSITION_CONTROL). + forces (float, list of float): in POSITION_CONTROL and VELOCITY_CONTROL, these are the maximum motor + forces used to reach the target values. In TORQUE_CONTROL these are the forces / torques to be applied + each simulation step. + kp (float, list of float): position (stiffness) gain(s) (used in POSITION_CONTROL). + kd (float, list of float): velocity (damping) gain(s) (used in POSITION_CONTROL). max_velocity (float): in POSITION_CONTROL this limits the velocity to a maximum. """ pass - def set_joint_motor_control_array(self, body_id, joint_ids, control_mode=2, positions=None, - velocities=None, forces=None, kps=None, kds=None): - """ - Instead of making individual calls for each joint, you can pass arrays for all inputs to reduce calling - overhead dramatically. - - Args: - body_id (int): body unique id. - joint_ids (list of int): list of joint id. - control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD), - VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3). - positions (list of float): list of target joint positions (used in POSITION_CONTROL) the target value is - target position of the joint. - velocities (list of float): list of target joint velocities (used in PD_CONTROL, VELOCITY_CONTROL and - POSITION_CONTROL). - forces (list of float): list of forces. In POSITION_CONTROL and VELOCITY_CONTROL, these are the maximum - motor forces used to reach the target values. In TORQUE_CONTROL these are the forces/torques to be - applied each simulation step. - kps (list of float): list of position (stiffness) gains (used in POSITION_CONTROL). - kds (list of float): list of velocity (damping) gains (used in POSITION_CONTROL). - """ - pass - def get_link_state(self, body_id, link_id, compute_velocity=False, compute_forward_kinematics=False): """ Get the state of the associated link. @@ -965,7 +1106,7 @@ class Simulator(object): def get_link_velocities(self, body_id, link_ids): pass - def get_qindex(self, body_id, joint_ids): + def get_q_indices(self, body_id, joint_ids): """ Get the corresponding q index of the given joint(s). @@ -985,9 +1126,6 @@ class Simulator(object): """ Get the actuated joint ids associated with the given body id. - Warnings: this checks through the list of all joints each time it is called. It might be a good idea to call - this method one time and cache the actuated joint ids. - Args: body_id (int): unique body id. @@ -1012,6 +1150,36 @@ class Simulator(object): """ pass + def get_joint_type_ids(self, body_id, joint_ids): + """ + Get the joint type ids. + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + int: joint type id. + if multiple joints: list of above + """ + pass + + def get_joint_type_names(self, body_id, joint_ids): + """ + Get joint type names. + + Args: + body_id (int): unique body id. + joint_ids (int, list of int): a joint id, or list of joint ids. + + Returns: + if 1 joint: + str: joint type name. + if multiple joints: list of above + """ + pass + def get_joint_dampings(self, body_id, joint_ids): """ Get the damping coefficient of the given joint(s). @@ -1040,7 +1208,7 @@ class Simulator(object): if 1 joint: float: friction coefficient of the given joint if multiple joints: - float[N]: friction coefficient for each specified joint + np.float[N]: friction coefficient for each specified joint """ pass @@ -1074,7 +1242,7 @@ class Simulator(object): if 1 joint: float: maximum force [N] if multiple joints: - float[N]: maximum force for each specified joint [N] + np.float[N]: maximum force for each specified joint [N] """ pass @@ -1301,24 +1469,25 @@ class Simulator(object): def get_visual_shape_data(self, object_id, flags=-1): """ - Get the visual shape data associated with the given object id. + Get the visual shape data associated with the given object id. It will output a list of visual shape data. Args: object_id (int): object unique id. flags (int, None): VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) will also provide `texture_unique_id`. Returns: - int: object unique id. - int: link index or -1 for the base - int: visual geometry type (TBD) - np.float[3]: dimensions (size, local scale) of the geometry - str: path to the triangle mesh, if any. Typically relative to the URDF, SDF or MJCF file location, but - could be absolute - np.float[3]: position of local visual frame, relative to link/joint frame - np.float[4]: orientation of local visual frame relative to link/joint frame - list of 4 floats: URDF color (if any specified) in Red / Green / Blue / Alpha - int: texture unique id of the shape or -1 if None. This field only exists if using - VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) flag. + list: + int: object unique id. + int: link index or -1 for the base + int: visual geometry type (TBD) + np.float[3]: dimensions (size, local scale) of the geometry + str: path to the triangle mesh, if any. Typically relative to the URDF, SDF or MJCF file location, but + could be absolute + np.float[3]: position of local visual frame, relative to link/joint frame + np.float[4]: orientation of local visual frame relative to link/joint frame + list of 4 floats: URDF color (if any specified) in Red / Green / Blue / Alpha + int: texture unique id of the shape or -1 if None. This field only exists if using + VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) flag. """ pass @@ -1625,16 +1794,16 @@ class Simulator(object): """ pass - def get_contact_points(self, body_a, body_b, link_id_a=None, link_id_b=None): + def get_contact_points(self, body1, body2, link1_id=None, link2_id=None): """ Returns the contact points computed during the most recent call to `step`. Args: - body_a (int): only report contact points that involve body A - body_b (int): only report contact points that involve body B. Important: you need to have a valid body A + body1 (int): only report contact points that involve body A + body2 (int): only report contact points that involve body B. Important: you need to have a valid body A if you provide body B - link_id_a (int): only report contact points that involve link index of body A - link_id_b (int): only report contact points that involve link index of body B + link1_id (int): only report contact points that involve link index of body A + link2_id (int): only report contact points that involve link index of body B Returns: list: @@ -1655,18 +1824,18 @@ class Simulator(object): """ pass - def get_closest_points(self, body_a, body_b, distance, link_id_a=None, link_id_b=None): + def get_closest_points(self, body1, body2, distance, link1_id=None, link2_id=None): """ Computes the closest points, independent from `step`. This also lets you compute closest points of objects with an arbitrary separating distance. In this query there will be no normal forces reported. Args: - body_a (int): only report contact points that involve body A - body_b (int): only report contact points that involve body B. Important: you need to have a valid body A + body1 (int): only report contact points that involve body A + body2 (int): only report contact points that involve body B. Important: you need to have a valid body A if you provide body B distance (float): If the distance between objects exceeds this maximum distance, no points may be returned. - link_id_a (int): only report contact points that involve link index of body A - link_id_b (int): only report contact points that involve link index of body B + link1_id (int): only report contact points that involve link index of body A + link2_id (int): only report contact points that involve link index of body B Returns: list: @@ -1696,18 +1865,19 @@ class Simulator(object): to_position (np.float[3]): end of the ray in world coordinates Returns: - int: object unique id of the hit object - int: link index of the hit object, or -1 if none/parent - float: hit fraction along the ray in range [0,1] along the ray. - np.float[3]: hit position in Cartesian world coordinates - np.float[3]: hit normal in Cartesian world coordinates + list: + int: object unique id of the hit object + int: link index of the hit object, or -1 if none/parent + float: hit fraction along the ray in range [0,1] along the ray. + np.float[3]: hit position in Cartesian world coordinates + np.float[3]: hit normal in Cartesian world coordinates """ pass def ray_test_batch(self, from_positions, to_positions, parent_object_id=None, parent_link_id=None): """Perform a batch of raycasts to find the intersection information of the first objects hit. - This is similar to the rayTest, but allows you to provide an array of rays, for faster execution. The size of + This is similar to the ray_test, but allows you to provide an array of rays, for faster execution. The size of 'rayFromPositions' needs to be equal to the size of 'rayToPositions'. You can one ray result per ray, even if there is no intersection: you need to use the objectUniqueId field to check if the ray has hit anything: if the objectUniqueId is -1, there is no hit. In that case, the 'hit fraction' is 1. @@ -1742,15 +1912,15 @@ class Simulator(object): """ pass - def set_collision_filter_pair(self, body_a, body_b, link_a=-1, link_b=-1, enable=True): + def set_collision_filter_pair(self, body1, body2, link1=-1, link2=-1, enable=True): """ Enable/disable collision between two bodies/links. Args: - body_a (int): unique id of body A to be filtered - body_b (int): unique id of body B to be filtered, A==B implies self-collision - link_a (int): link index of body A - link_b (int): link index of body B + body1 (int): unique id of body A to be filtered + body2 (int): unique id of body B to be filtered, A==B implies self-collision + link1 (int): link index of body A + link2 (int): link index of body B enable (bool): True to enable collision, False to disable collision """ pass @@ -1891,7 +2061,7 @@ class Simulator(object): solver (int): p.IK_DLS (=0) or p.IK_SDLS (=1), Damped Least Squares or Selective Damped Least Squares, as described in the paper by Samuel Buss "Selectively Damped Least Squares for Inverse Kinematics". q_curr (np.float[N]): list of joint positions. By default PyBullet uses the joint positions of the body. - If provided, the targetPosition and targetOrientation is in local space! + If provided, the target_position and targetOrientation is in local space! max_iters (int): maximum number of iterations. Refine the IK solution until the distance between target and actual end effector position is below this threshold, or the `max_iters` is reached. threshold (float): residual threshold. Refine the IK solution until the distance between target and actual @@ -1987,7 +2157,7 @@ class Simulator(object): torques (np.float[N]): desired joint torques Returns: - float[N]: joint accelerations computed using the rigid-body equation of motion + np.float[N]: joint accelerations computed using the rigid-body equation of motion References: [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1 diff --git a/pyrobolearn/states/basic_states.py b/pyrobolearn/states/basic_states.py index 4ccaee1..fe760de 100644 --- a/pyrobolearn/states/basic_states.py +++ b/pyrobolearn/states/basic_states.py @@ -7,7 +7,7 @@ This includes notably the fixed, functional, and counter states. import numpy as np from pyrobolearn.states.state import State -from pyrobolearn.actions.action import Action +from pyrobolearn.actions import Action __author__ = "Brian Delhaisse" @@ -108,9 +108,9 @@ if __name__ == '__main__': print("\nSome dimensions:") print("s.shape: {}".format(s.shape)) print("s.dimension: {}".format(s.dimension)) - print("s.maxDimension: {}".format(s.maxDimension())) + print("s.max_dimension: {}".format(s.max_dimension())) print("s.size: {}".format(s.size)) - print("s.totalSize: {}".format(s.totalSize())) + print("s.total_size: {}".format(s.total_size())) print("len(s) = {}".format(len(s))) print("len(s1) = {}".format(len(s1))) # print(s2 + s1) diff --git a/pyrobolearn/states/body_states.py b/pyrobolearn/states/body_states.py index 7e4c319..962fb01 100644 --- a/pyrobolearn/states/body_states.py +++ b/pyrobolearn/states/body_states.py @@ -26,19 +26,27 @@ class BodyState(State): """ __metaclass__ = ABCMeta - def __init__(self, obj, world=None): + def __init__(self, body, world=None): + """ + Initialize the body state. + + Args: + body (Body, int): body or unique body id. + world (None, World): world instance if the body id was given. + """ + super(BodyState, self).__init__() - if not isinstance(obj, (Body, int)): + if not isinstance(body, (Body, int)): raise TypeError("Expecting an instance of Body, or an identifier from the simulator/world.") - if isinstance(obj, int): + if isinstance(body, int): if not isinstance(world, World): # try to look for the world in global variables - if 'world' in globals() and isinstance(globals()['world'], World): # O(1) + if 'world' in globals() and isinstance(globals()['world'], World): # O(1) world = globals()['world'] else: - raise ValueError("When giving the object identifier, the world need to be given as well.") - obj = Body(world.simulator, obj) - self.obj = obj + raise ValueError("When giving the body identifier, the world need to be given as well.") + body = Body(world.simulator, body) + self.body = body @abstractmethod def _read(self): @@ -46,33 +54,57 @@ class BodyState(State): class PositionState(BodyState): - """Position of an object. + """Position of a body in the world. """ - def __init__(self, obj, world=None): - super(PositionState, self).__init__(obj, world) - self.data = self.obj.position + + def __init__(self, body, world=None): + """ + Initialize the position state. + + Args: + body (Body, int): body or unique body id. + world (None, World): world instance if the body id was given. + """ + super(PositionState, self).__init__(body, world) + self.data = self.body.position def _read(self): - self.data = self.obj.position + self.data = self.body.position class OrientationState(BodyState): - """Orientation of an object. + """Orientation of a body in the world. """ - def __init__(self, obj, world=None): - super(OrientationState, self).__init__(obj, world) - self.data = self.obj.orientation + + def __init__(self, body, world=None): + """ + Initialize the orientation state. + + Args: + body (Body, int): body or unique body id. + world (None, World): world instance if the body id was given. + """ + super(OrientationState, self).__init__(body, world) + self.data = self.body.orientation def _read(self): - self.data = self.obj.orientation + self.data = self.body.orientation class VelocityState(BodyState): - """Velocity of an object. + """Velocity of a body in the world """ - def __init__(self, obj, world=None): - super(VelocityState, self).__init__(obj, world) - self.data = self.obj.velocity + + def __init__(self, body, world=None): + """ + Initialize the velocity state. + + Args: + body (Body, int): body or unique body id. + world (None, World): world instance if the body id was given. + """ + super(VelocityState, self).__init__(body, world) + self.data = self.body.velocity def _read(self): - self.data = self.obj.velocity + self.data = self.body.velocity diff --git a/pyrobolearn/states/robot_states/joint_states.py b/pyrobolearn/states/robot_states/joint_states.py index 00d09d2..c597cfa 100644 --- a/pyrobolearn/states/robot_states/joint_states.py +++ b/pyrobolearn/states/robot_states/joint_states.py @@ -6,7 +6,7 @@ This includes notably the joint positions, velocities, and force/torque states. from abc import ABCMeta -from pyrobolearn.states.robot_states import RobotState +from pyrobolearn.states.robot_states.robot_states import RobotState __author__ = "Brian Delhaisse" @@ -36,7 +36,7 @@ class JointState(RobotState): # get the joints of the robot if joint_ids is None: - joint_ids = robot.getJointIds() + joint_ids = robot.get_joint_ids() elif isinstance(joint_ids, int): joint_ids = [joint_ids] self.joints = joint_ids @@ -55,7 +55,7 @@ class JointPositionState(JointState): super(JointPositionState, self).__init__(robot, joint_ids) def _read(self): - self._data = self.robot.getJointPositions(self.joints) + self.data = self.robot.get_joint_positions(self.joints) class JointVelocityState(JointState): @@ -68,7 +68,7 @@ class JointVelocityState(JointState): super(JointVelocityState, self).__init__(robot, joint_ids) def _read(self): - self._data = self.robot.getJointVelocities(self.joints) + self.data = self.robot.get_joint_velocities(self.joints) class JointForceTorqueState(JointState): @@ -81,7 +81,7 @@ class JointForceTorqueState(JointState): super(JointForceTorqueState, self).__init__(robot, joint_ids) def _read(self): - self._data = self.robot.getJointTorques(self.joints) + self.data = self.robot.get_joint_torques(self.joints) class JointAccelerationState(JointState): @@ -95,4 +95,4 @@ class JointAccelerationState(JointState): super(JointAccelerationState, self).__init__(robot, joint_ids) def _read(self): - self._data = self.robot.getJointAccelerations(self.joints) + self.data = self.robot.get_joint_accelerations(self.joints) diff --git a/pyrobolearn/states/robot_states/link_states.py b/pyrobolearn/states/robot_states/link_states.py index fb0f743..d54089f 100644 --- a/pyrobolearn/states/robot_states/link_states.py +++ b/pyrobolearn/states/robot_states/link_states.py @@ -6,7 +6,7 @@ This includes notably the link positions and velocities. from abc import ABCMeta -from pyrobolearn.states.robot_states import RobotState +from pyrobolearn.states.robot_states.robot_states import RobotState __author__ = "Brian Delhaisse" @@ -36,7 +36,7 @@ class LinkState(RobotState): # get links from robot if link_ids is None: - link_ids = robot.getlink_ids() + link_ids = range(robot.num_links) self.links = link_ids # read the data @@ -52,7 +52,7 @@ class LinkPositionState(LinkState): super(LinkPositionState, self).__init__(robot, link_ids) def _read(self): - self._data = self.robot.getLinkPositions(self.links, wrtLinkId=self.wrt_link_id) + self.data = self.robot.get_link_positions(self.links, wrt_link_id=self.wrt_link_id) class LinkWorldPositionState(LinkState): @@ -63,7 +63,7 @@ class LinkWorldPositionState(LinkState): super(LinkWorldPositionState, self).__init__(robot, link_ids) def _read(self): - self._data = self.robot.getLinkPositions(self.links) + self.data = self.robot.get_link_positions(self.links) class LinkOrientationState(LinkState): @@ -74,7 +74,7 @@ class LinkOrientationState(LinkState): super(LinkOrientationState, self).__init__(robot, link_ids) def _read(self): # TODO: convert - self._data = self.robot.getLinkOrientations(self.links) + self.data = self.robot.get_link_orientations(self.links) class LinkVelocityState(LinkState): @@ -85,7 +85,7 @@ class LinkVelocityState(LinkState): super(LinkVelocityState, self).__init__(robot, link_ids) def _read(self): - self._data = self.robot.getLinkVelocities(self.links) + self.data = self.robot.get_link_velocities(self.links) class LinkLinearVelocityState(LinkState): @@ -96,7 +96,7 @@ class LinkLinearVelocityState(LinkState): super(LinkLinearVelocityState, self).__init__(robot, link_ids) def _read(self): - self._data = self.robot.getLinkLinearVelocities(self.links) + self.data = self.robot.get_link_linear_velocities(self.links) class LinkAngularVelocityState(LinkState): @@ -107,4 +107,4 @@ class LinkAngularVelocityState(LinkState): super(LinkAngularVelocityState, self).__init__(robot, link_ids) def _read(self): - self._data = self.robot.getLinkAngularVelocities(self.links) + self.data = self.robot.get_link_angular_velocities(self.links) diff --git a/pyrobolearn/states/robot_states/robot_states.py b/pyrobolearn/states/robot_states/robot_states.py index adf4b5b..2368169 100644 --- a/pyrobolearn/states/robot_states/robot_states.py +++ b/pyrobolearn/states/robot_states/robot_states.py @@ -65,7 +65,7 @@ class BasePositionState(RobotState): self._read() def _read(self): - self._data = self.robot.getBasePosition() + self.data = self.robot.get_base_position() class BaseHeightState(RobotState): @@ -79,7 +79,7 @@ class BaseHeightState(RobotState): self._read() def _read(self): - self._data = np.array([self.robot.getBasePosition()[-1]]) + self.data = np.array([self.robot.get_base_position()[-1]]) class BaseOrientationState(RobotState): @@ -93,7 +93,7 @@ class BaseOrientationState(RobotState): self._read() def _read(self): - self._data = self.robot.getBaseOrientation(convert_to_numpy_quaternion=False) + self.data = self.robot.get_base_orientation() class BaseLinearVelocityState(RobotState): @@ -107,7 +107,7 @@ class BaseLinearVelocityState(RobotState): self._read() def _read(self): - self._data = self.robot.getBaseLinearVelocity() + self.data = self.robot.get_base_linear_velocity() class BaseAngularVelocityState(RobotState): @@ -121,4 +121,4 @@ class BaseAngularVelocityState(RobotState): self._read() def _read(self): - self._data = self.robot.getBaseAngularVelocity() + self.data = self.robot.get_base_angular_velocity() diff --git a/pyrobolearn/states/robot_states/sensor_states.py b/pyrobolearn/states/robot_states/sensor_states.py index e567f3f..fa50e3a 100644 --- a/pyrobolearn/states/robot_states/sensor_states.py +++ b/pyrobolearn/states/robot_states/sensor_states.py @@ -6,7 +6,7 @@ This includes notably the camera, contact, IMU, force/torque sensors and others. from abc import ABCMeta -from pyrobolearn.states.robot_states import RobotState +from pyrobolearn.states.robot_states.robot_states import RobotState __author__ = "Brian Delhaisse" @@ -62,4 +62,4 @@ class FeetContactState(ContactState): super(FeetContactState, self).__init__(robot, contacts) def _read(self): - pass \ No newline at end of file + pass diff --git a/pyrobolearn/states/state_processor.py b/pyrobolearn/states/state_processor.py new file mode 100644 index 0000000..a861425 --- /dev/null +++ b/pyrobolearn/states/state_processor.py @@ -0,0 +1,304 @@ +#!/usr/bin/env python +"""Provide state processors + +The various state processors defined here allow to process the data after being read. In contrast, to process the +data for each approximators (policy, value function, etc), you can process the state once here. However, it might +not always be beneficial to process it; other part of the code might require the real original state and not the +processed one. +""" + +# TODO: correct the few mistakes! + +import numpy as np + +from pyrobolearn.states.state import State + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "MIT" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class StateProcessor(State): + r"""State Processor + + The state processor allows to pre-process the data by first reading it and then process it. + """ + + def __init__(self, state): + """ + Initialize the state processor. + + Args: + state (State): state to process. + """ + super(StateProcessor, self).__init__() + self.state = state + + def read(self): + data = self.state() + return data + + +class ShiftStateProcessor(StateProcessor): + r"""Shift State Processor + + Shift the data by the given amount; that is, it returned: :math:`\hat{x} = x + z` where :math:`z` is the + specified amount to shift the original input :math:`x`. + """ + + def __init__(self, state, z): + """ + Initialize the Shift Processor. + + Args: + state (State): state to process. + z (int, float, np.array, torch.Tensor): amount to be shifted. + """ + super(ShiftStateProcessor, self).__init__(state) + if isinstance(z, (int, float)): + z = [z] + self.z = np.array(z) + + def read(self): + data = self.state() + return data - self.z + + +class RunningCenterStateProcessor(StateProcessor): + r"""Running Center State Processor + + Center the data by using the mean which is updated each time a new data point is given. + """ + + def __init__(self, state): + super(RunningCenterStateProcessor, self).__init__(state) + self.mean = 0. + self.N = 0 + + def reset(self): + self.mean = 0. + self.N = 0 + + def read(self): + x = self.state() + # update the mean + self.mean = self.N / (self.N + 1.) * self.mean + 1. / (self.N + 1) * x + self.N += 1 + + # center the data with new mean + return x - self.mean + + +class StandardizerStateProcessor(StateProcessor): + r"""Standardizer State Processor + + Processor that standardizes the given data; the returned data is centered around 0 with a standard deviation of 1. + That is, it returned :math:`\hat{x} = \frac{x - \mu}{\sigma}`, where :math:`\mu` is the mean, and :math:`\sigma` + is the standard deviation. + """ + + def __init__(self, state, mean=0., std_dev=1., epsilon=1.e-4): + """ + Initialize the Standardizer Processor. + + Args: + state (State): state to process. + mean (int, float, np.array, torch.Tensor): mean + std_dev (int, float, np.array, torch.Tensor): standard deviation + epsilon (float): small number to be added to the denominator for stability in case the std dev = 0 + """ + super(StandardizerStateProcessor, self).__init__(state) + if isinstance(mean, (int, float)): + mean = [mean] + if isinstance(std_dev, (int, float)): + std_dev = [std_dev] + self.mean = np.array(mean) + self.std = np.array(std_dev) + self.eps = epsilon + + def read(self): + x = self.state() + return (x - self.mean) / (self.std + self.eps) + + +class RunningStandardizerStateProcessor(StateProcessor): + r"""Running Standardizer State Processor + + Processor that standardizes the given data; the returned data is centered around 0 with a standard deviation of 1. + That is, it returned :math:`\hat{x} = \frac{x - \mu}{\sigma}`, where :math:`\mu` is the mean, and :math:`\sigma` + is the standard deviation. The mean and the standard deviation (or variance) are updated at each time a new data + point is given. + """ + + def __init__(self, state, epsilon=1.e-4): + """ + Initialize the Running Standardizer Processor. + + Args: + state (State): state to process. + epsilon (float): small number to be added to the denominator for stability in case the std dev = 0 + """ + super(RunningStandardizerStateProcessor, self).__init__(state) + self.mean = 0. + self.var = 1. + self.N = 0 + self.eps = epsilon + + def reset(self): + self.mean = 0. + self.var = 1. + self.N = 0 + + def read(self): + x = self.state() + # update the mean + old_mean = np.copy(self.mean) + self.mean = self.N / (self.N + 1.) * self.mean + 1. / (self.N + 1) * x + + # update the var / stddev + self.var = self.N / (self.N + 1) * self.var + 1. / (self.N + 1) * (x - old_mean) * (x - self.mean) + std = np.sqrt(self.var) + + # update total number of data points + self.N += 1 + + # standardize the data + return (x - self.mean) / (std + self.eps) + + +class NormalizerStateProcessor(StateProcessor): + r"""Normalizer State Processor + + Processor that normalizes the given data; the returned data will be between 0 and 1. + That is, it returned :math:`\hat{x} = \frac{x - x_{min}}{x_{max} - x_{min}}`, where + :math:`x \in [x_{min}, x_{max}]`. + """ + + def __init__(self, state, xmin, xmax): + """ + Initialize the Normalizer Processor. + + Args: + state (State): state to process. + xmin (int, float, np.array, torch.Tensor): minimum bound + xmax (int, float, np.array, torch.Tensor): maximum bound + """ + super(NormalizerStateProcessor, self).__init__(state) + if isinstance(xmin, (int, float)): + xmin = [xmin] + if isinstance(xmax, (int, float)): + xmax = [xmax] + self.xmin = np.array(xmin) + self.xmax = np.array(xmax) + if torch.allclose(self.xmin, self.xmax): + raise ValueError("The given arguments 'xmin' and 'xmax' are the same.") + + def read(self): + x = self.state() + return (x - self.xmin) / (self.xmax - self.xmin) + + +class RunningNormalizerStateProcessor(StateProcessor): + r"""Running Normalizer State Processor + + Processor that normalizes the given data; the returned data will be between 0 and 1. + That is, it returned :math:`\hat{x} = \frac{x - x_{min}}{x_{max} - x_{min}}`, where + :math:`x \in [x_{min}, x_{max}]`. The :math:`x_{min}` and `x_{max}` will be updated each time a new data point + is given. + + Warnings: it will return zero at the beginning as x = x_min = x_max. + """ + + def __init__(self, state): + super(RunningNormalizerStateProcessor, self).__init__(state) + self.xmin = 0. + self.xmax = 1. + + def reset(self): + self.xmin = 0. + self.xmax = 1. + + def read(self): + x = self.state() + # compute new xmin and xmax given new data point + self.xmin = np.minimum(x, self.xmin) + self.xmax = np.maximum(x, self.xmax) + + # if xmax and xmin are not different, make the denominator to be 1 + idx = (self.xmax == self.xmin) + self.xmax[idx] = self.xmin[idx] + 1. + + # normalize + return (x - self.xmin) / (self.xmax - self.xmin) + + +class ClipStateProcessor(StateProcessor): + r"""Clip State Processor + + Processor that clips the given data; the returned data will be between [low, high], where `low` and `high` are + respectively the specified lower and higher bound. + """ + + def __init__(self, state, low=-10., high=10.): + """ + Initialize the Clip processor. + + Args: + state (State): state to process. + low (int, float, np.array, torch.Tensor): lower bound + high (int, float, np.array, torch.Tensor): higher bound + """ + super(ClipStateProcessor, self).__init__(state) + if isinstance(low, (int, float)): + low = [low] + if isinstance(high, (int, float)): + high = [high] + self.low = np.array(low) + self.high = np.array(high) + + def read(self): + x = self.state() + return np.clip(x, self.low, self.high) + + +class ScaleStateProcessor(StateProcessor): + r"""Scale processor + + Processor that scales the input x which is between [x1, x2] to the output y which is between [y1, y2]. + This is for instance useful after a tanh layer in a neural network which outputs a value between -1 and 1, and + that value has to be rescaled to a bigger (absolute) value. + """ + + def __init__(self, state, x1, x2, y1, y2): + """ + Initialize the scale processor. + + Args: + state (State): state to process. + x1 (int, float, np.array, torch.Tensor): lower bound of input + x2 (int, float, np.array, torch.Tensor): upper bound of input + y1 (int, float, np.array, torch.Tensor): lower bound of output + y2 (int, float, np.array, torch.Tensor): upper bound of output + """ + + super(ScaleStateProcessor, self).__init__(state) + + def convert(x): + if isinstance(x, (int, float)): + return [x] + return x + + self.x1 = np.array(convert(x1)) + self.x2 = np.array(convert(x2)) + self.y1 = np.array(convert(y1)) + self.y2 = np.array(convert(y2)) + self.ratio = (self.y2 - self.y1) / (self.x2 - self.x1) + + def read(self): + x = self.state() + return self.y1 + (x - self.x1) * self.ratio diff --git a/pyrobolearn/tools/bridges/audio/robots/bridge_speech_wheeled.py b/pyrobolearn/tools/bridges/audio/robots/bridge_speech_wheeled.py index 3049dd7..94351c0 100644 --- a/pyrobolearn/tools/bridges/audio/robots/bridge_speech_wheeled.py +++ b/pyrobolearn/tools/bridges/audio/robots/bridge_speech_wheeled.py @@ -28,9 +28,9 @@ class BridgeSpeechRecognizerWheeledRobot(Bridge): if data == 'stop': self.robot.stop() elif data == 'move forward': - self.robot.driveForward(self.speed) + self.robot.drive_forward(self.speed) elif data == 'move backward': - self.robot.driveBackward(self.speed) + self.robot.drive_backward(self.speed) elif data == 'turn right': pass elif data == 'turn left': @@ -67,13 +67,13 @@ class BridgeSpeechRecognizerAckermannWheeledRobot(Bridge): if data == 'stop': self.robot.stop() elif data == 'move forward': - self.robot.driveForward(self.speed) + self.robot.drive_forward(self.speed) elif data == 'move backward': - self.robot.driveBackward(self.speed) + self.robot.drive_backward(self.speed) elif data == 'turn right': - self.robot.setSteering(np.deg2rad(-20)) + self.robot.set_steering(np.deg2rad(-20)) elif data == 'turn left': - self.robot.setSteering(np.deg2rad(20)) + self.robot.set_steering(np.deg2rad(20)) elif data == 'faster': self.speed *= 2. elif data == 'slower': diff --git a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py index 826ad8f..d9674fa 100644 --- a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py +++ b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py @@ -66,8 +66,8 @@ class BridgeXboxOneAckermannWheeledRobot(Bridge): def step(self): x,y = self.interface.LJ - self.robot.setSteering(-x / 2.) - self.robot.driveForward(y * self.speed) + self.robot.set_steering(-x / 2.) + self.robot.drive_forward(y * self.speed) if self.interface.A: print('increasing speed +1') diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py index 1721263..d70da4d 100644 --- a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py +++ b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py @@ -49,7 +49,7 @@ class BridgeMouseKeyboardWorld(Bridge): * `w`: show the wireframe (collision shapes) * `s`: show the reference system * `v`: show bounding boxes - * `g`: show/hide parts of the GUI the side columns (check `sim.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)`) + * `g`: show/hide parts of the GUI the side columns (check `sim.configure_debug_visualizer(p.COV_ENABLE_GUI, 0)`) * `esc`: quit the simulator * `x`: change camera view such that it is perpendicular to the x-axis * `y`: change camera view such that it is perpendicular to the y-axis @@ -166,7 +166,7 @@ class BridgeMouseKeyboardWorld(Bridge): @property def camera(self): if self._camera is None: - self._camera = self.world_camera.getDebugVisualizerCamera(convert=False) + self._camera = self.world_camera.get_debug_visualizer_camera(convert=False) if self.default_camera is None: self.default_camera = self._camera return self._camera @@ -196,7 +196,7 @@ class BridgeMouseKeyboardWorld(Bridge): # update joint sliders if present (position control) if self.joint_sliders: - self.robot.updateJointSlider() + self.robot.update_joint_slider() # update task sliders if present (IK) if self.task_sliders: @@ -213,30 +213,27 @@ class BridgeMouseKeyboardWorld(Bridge): """Change camera view X.""" self.print_debug('change camera view X') dist, target = self.camera[-2:] - self.simulator.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=90., cameraPitch=0., - cameraTargetPosition=target) + self.simulator.reset_debug_visualizer(distance=dist, yaw=np.deg2rad(90.), pitch=0., target_position=target) def change_camera_view_y(self): """Change camera view Y.""" self.print_debug('change camera view Y') dist, target = self.camera[-2:] - self.simulator.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=180., cameraPitch=0., - cameraTargetPosition=target) + self.simulator.reset_debug_visualizer(distance=dist, yaw=np.deg2rad(180.), pitch=0., target_position=target) def change_camera_view_z(self): """Change camera view Z.""" self.print_debug('change camera view Z') dist, target = self.camera[-2:] - self.simulator.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=-90., cameraPitch=-89.99, - cameraTargetPosition=target) + self.simulator.reset_debug_visualizer(distance=dist, yaw=-np.deg2rad(90.), pitch=-np.deg2rad(89.99), + target_position=target) def reset_camera_view(self): """Reset camera view.""" self.print_debug('reset camera view') if self.default_camera is not None: yaw, pitch, dist, target = self.default_camera[-4:] - self.simulator.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=yaw, cameraPitch=pitch, - cameraTargetPosition=target) + self.simulator.reset_debug_visualizer(distance=dist, yaw=yaw, pitch=pitch, target_position=target) self.default_camera = None def pause(self): @@ -247,23 +244,23 @@ class BridgeMouseKeyboardWorld(Bridge): def gui(self): """show/hide GUI.""" self.hiding_gui = not self.hiding_gui - self.simulator.configureDebugVisualizer(self.simulator.COV_ENABLE_GUI, self.hiding_gui) + self.simulator.configure_debug_visualizer(self.simulator.COV_ENABLE_GUI, self.hiding_gui) self.print_debug('hide the GUI', 'enable the GUI', self.hiding_gui) def reset_world(self): """Reset the world.""" self.print_debug('reset the world') - self.world.resetRobots() + self.world.reset_robots() self.simulator.removeAllUserDebugItems() def update_joint_sliders(self): """Update joint sliders.""" if self.robot is not None: if self.joint_sliders: # remove joint sliders - self.robot.removeJointSlider() + self.robot.remove_joint_slider() self.print_debug('remove joint sliders') else: # add joint sliders - self.robot.addJointSlider() + self.robot.add_joint_slider() self.print_debug('add joint sliders') self.joint_sliders = not self.joint_sliders @@ -272,14 +269,14 @@ class BridgeMouseKeyboardWorld(Bridge): if self.robot is not None and self.link_id is not None: if self.link_id in self.task_sliders: # remove task sliders for idx in self.task_sliders[self.link_id]: - self.simulator.removeUserDebugItem(self.task_sliders[self.link_id][idx]) + self.simulator.remove_user_debug_item(self.task_sliders[self.link_id][idx]) self.task_sliders.pop(self.link_id) self.print_debug('remove task sliders') else: # add task sliders self.task_sliders[self.link_id] = {} - pos = self.robot.getLinkWorldPositions(self.link_id) + pos = self.robot.get_link_world_positions(self.link_id) for i, name in zip(pos, ['x', 'y', 'z']): - slider = self.simulator.addUserDebugParameter(name, i - 2., i + 2., i) + slider = self.simulator.add_user_debug_parameter(name, i - 2., i + 2., i) self.task_sliders[self.link_id][name] = slider self.print_debug('add task sliders') @@ -291,14 +288,14 @@ class BridgeMouseKeyboardWorld(Bridge): def add_world_text(self, string, position, color=(0.,0.,0.), size=1., lifetime=0.): """Add world text.""" self.print_debug('add world text') - self.simulator.addUserDebugText(string, position, color, size, lifetime) + self.simulator.add_user_debug_text(string, position, color, size, lifetime) def add_screen_text(self, string, world_position, color=(0.,0.,0.), size=1., lifetime=0.): """Add screen text.""" self.print_debug('add screen text') - V, P, Vp, V_inv, P_inv, Vp_inv = self.world_camera.getMatrices(True) - position = self.world_camera.screenToWorld(world_position, Vp_inv, P_inv, V_inv)[:3] - self.simulator.addUserDebugText(string, position, color, size, lifetime) + V, P, Vp, V_inv, P_inv, Vp_inv = self.world_camera.get_matrices(True) + position = self.world_camera.screen_to_world(world_position, Vp_inv, P_inv, V_inv)[:3] + self.simulator.add_user_debug_text(string, position, color, size, lifetime) def check_key_events(self): # call function corresponding to key combination @@ -314,20 +311,20 @@ class BridgeMouseKeyboardWorld(Bridge): # check what object we are trying to grab with the mouse by checking collision if self.interface.mouse_pressed: - V, P, Vp, V_inv, P_inv, Vp_inv = self.world_camera.getMatrices(True) - camera = self.world_camera.getDebugVisualizerCamera(convert=False) + V, P, Vp, V_inv, P_inv, Vp_inv = self.world_camera.get_matrices(True) + camera = self.world_camera.get_debug_visualizer_camera() # from the point (x,y) on the screen, get nearest and farthest point on the screen x_screen_init = np.array([self.interface.mouse_x, self.interface.mouse_y, 1., 1.]) x_screen_final = np.array([self.interface.mouse_x, self.interface.mouse_y, 0., 1.]) # get the corresponding points in the world - x_world_init = self.world_camera.screenToWorld(x_screen_init, Vp_inv, P_inv, V_inv) - x_world_final = self.world_camera.screenToWorld(x_screen_final, Vp_inv, P_inv, V_inv) + x_world_init = self.world_camera.screen_to_world(x_screen_init, Vp_inv, P_inv, V_inv) + x_world_final = self.world_camera.screen_to_world(x_screen_final, Vp_inv, P_inv, V_inv) # check if there is a collision # print(x_world_init[:3], x_world_final[:3]) - collision = self.simulator.rayTest(list(x_world_init[:3]), list(x_world_final[:3])) + collision = self.simulator.ray_test(list(x_world_init[:3]), list(x_world_final[:3])) # if collision, proceed the inverse operation to get the depth on the screen if len(collision) > 0: @@ -337,25 +334,25 @@ class BridgeMouseKeyboardWorld(Bridge): # basePosition=list(x_world_init[:3])) # bodyId = self.simulator.createMultiBody(baseMass=0, baseVisualShapeIndex=self.vs2, # basePosition=list(x_world_final[:3])) - if object_id != -1 and self.world.isRobotId(object_id): # valid object + if object_id != -1 and self.world.is_robot_id(object_id): # valid object # Set robot and link_id - self.robot, self.link_id = self.world.getRobot(object_id), link_id + self.robot, self.link_id = self.world.get_robot(object_id), link_id width, height = camera[:2] x_screen = np.array([width/2, height/10, 0.95, 1]) - pos = self.world_camera.screenToWorld(x_screen, Vp_inv, P_inv, V_inv)[:3] - # self.simulator.addUserDebugText(str(self.robot) + ": " + self.robot.getLinkNames(self.link_id), + pos = self.world_camera.screen_to_world(x_screen, Vp_inv, P_inv, V_inv)[:3] + # self.simulator.add_user_debug_text(str(self.robot) + ": " + self.robot.get_link_names(self.link_id), # pos, RGBColor.black, textSize=1) # calculate plane # 1. compute the initial point on the plane (collision point) if link_id == -1: # no link - x0 = np.array(self.simulator.getBasePositionAndOrientation(object_id)[0]) + x0 = np.array(self.simulator.get_base_pose(object_id)[0]) else: # link - x0 = np.array(self.simulator.getLinkState(object_id, link_id)[0]) + x0 = np.array(self.simulator.get_link_state(object_id, link_id)[0]) - # 2. calculate normal (=targetPosition - eyePosition) to the plane + # 2. calculate normal (=target_position - eyePosition) to the plane # normal = x_world_final[:3] - x_world_init[:3] yaw, pitch, dist, target = camera[-4:] yaw, pitch = np.deg2rad(yaw), np.deg2rad(pitch) @@ -368,18 +365,18 @@ class BridgeMouseKeyboardWorld(Bridge): # calculate associate depth on the screen (because perspective projection) hit_pos = np.array(list(hit_pos) + [1.]) - self.depth = self.world_camera.worldToScreen(hit_pos, V, P, Vp)[2] + self.depth = self.world_camera.world_to_screen(hit_pos, V, P, Vp)[2] elif self.interface.mouse_down and self.interface.mouse_moving and self.robot is not None: - V, P, Vp, V_inv, P_inv, Vp_inv = self.world_camera.getMatrices(True) - camera = self.world_camera.getDebugVisualizerCamera(convert=False) + V, P, Vp, V_inv, P_inv, Vp_inv = self.world_camera.get_matrices(True) + camera = self.world_camera.get_debug_visualizer_camera(convert=False) if self.plane is not None: # project the point on the screen to the world, and check where the line that starts from this point # and is perpendicular to the plane (i.e. parallel to the normal) intersects with the aforementioned # plane x_screen = np.array([self.interface.mouse_x, self.interface.mouse_y, self.depth, 1]) - x_world = self.world_camera.screenToWorld(x_screen, Vp_inv, P_inv, V_inv)[:3] + x_world = self.world_camera.screen_to_world(x_screen, Vp_inv, P_inv, V_inv)[:3] point = self.plane.getIntersectionPoint(x_world) # # draw some spheres on the plane @@ -398,9 +395,9 @@ class BridgeMouseKeyboardWorld(Bridge): # RGBColor.red, 1., 2.) # # perform inverse kinematics - # q = self.robot.calculateInverseKinematics(self.link, point) + # q = self.robot.calculate_inverse_kinematics(self.link, point) # for i in range(self.robot.getNumberOfJoints()): - # self.robot.setJointPositions(i, q[i]) + # self.robot.set_joint_positions(i, q[i]) # Tests @@ -413,20 +410,20 @@ if __name__ == '__main__': # create simulator sim = BulletSim() - # sim.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) - # sim.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) - # sim.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 1) - # sim.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 1) - # sim.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 0) - # sim.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) - # sim.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) - # sim.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) + # sim.configure_debug_visualizer(p.COV_ENABLE_GUI, 0) + # sim.configure_debug_visualizer(p.COV_ENABLE_RENDERING, 0) + # sim.configure_debug_visualizer(p.COV_ENABLE_TINY_RENDERER, 1) + # sim.configure_debug_visualizer(p.COV_ENABLE_WIREFRAME, 1) + # sim.configure_debug_visualizer(p.COV_ENABLE_Y_AXIS_UP, 0) + # sim.configure_debug_visualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) + # sim.configure_debug_visualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) + # sim.configure_debug_visualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) # create World world = BasicWorld(sim) # load robot - robot = world.loadRobot('baxter', useFixedBase=True) + robot = world.load_robot('baxter', fixed_base=True) # create bridge/interface bridge = BridgeMouseKeyboardWorld(world, verbose=True) diff --git a/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py b/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py index 16c74d9..65dbeaf 100644 --- a/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py +++ b/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py @@ -16,7 +16,7 @@ from pyrobolearn.simulators import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "MIT" +__license__ = "(c) Brian Delhaisse" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" @@ -144,7 +144,7 @@ class MouseKeyboardInterface(InputInterface): def check_key_events(self): # get key events - events = self.simulator.getKeyboardEvents() + events = self.simulator.get_keyboard_events() # create new list of key pressed self.key_pressed = [] @@ -162,7 +162,7 @@ class MouseKeyboardInterface(InputInterface): def check_mouse_events(self): # get mouse events - events = self.simulator.getMouseEvents() + events = self.simulator.get_mouse_events() # reset mouse events self.mouse_moving, self.mouse_pressed = False, False diff --git a/pyrobolearn/tools/interfaces/vr/oculus.py b/pyrobolearn/tools/interfaces/vr/oculus.py index 573ef72..e25a475 100644 --- a/pyrobolearn/tools/interfaces/vr/oculus.py +++ b/pyrobolearn/tools/interfaces/vr/oculus.py @@ -65,14 +65,14 @@ class OculusInterface(VRInterface): self.task = None # create visual spheres in the world for the hands - self.worldCamera = self.world.getMainCamera() - V, P, Vp, V_inv, P_inv, Vp_inv = self.worldCamera.getMatrices(True) - camera = self.worldCamera.getDebugVisualizerCamera(convert=False) + self.worldCamera = self.world.get_main_camera() + V, P, Vp, V_inv, P_inv, Vp_inv = self.worldCamera.get_matrices(True) + camera = self.worldCamera.get_debug_visualizer_camera(convert=False) width, height = camera[:2] posL = np.array([width / 2 - 20, height / 2, 0.95, 1]) posR = np.array([width / 2 + 20, height / 2, 0.95, 1]) - posL = self.worldCamera.screenToWorld(posL, Vp_inv, P_inv, V_inv)[:3] - posR = self.worldCamera.screenToWorld(posR, Vp_inv, P_inv, V_inv)[:3] + posL = self.worldCamera.screen_to_world(posL, Vp_inv, P_inv, V_inv)[:3] + posR = self.worldCamera.screen_to_world(posR, Vp_inv, P_inv, V_inv)[:3] self.leftSphere = self.world.loadVisualSphere(posL, radius=0.1, color=RGBAColor.red) # red self.rightSphere = self.world.loadVisualSphere(posR, radius=0.1, color=RGBAColor.blue) # blue @@ -137,9 +137,9 @@ class OculusInterface(VRInterface): self.rightJoystick = [values[0], values[1], values[-2:]] # move the camera by rotating yaw, pitch = values[-2:] - #self.worldCamera.addYawPitch(yaw, pitch, radian=False) + #self.worldCamera.add_yaw_pitch(yaw, pitch, radian=False) #print(pitch, yaw) - pos = self.worldCamera.targetPosition + pos = self.worldCamera.target_position dist = self.worldCamera.dist elif name == 'BA': # button A: [touch, button] pass @@ -163,8 +163,8 @@ class OculusInterface(VRInterface): ### update world ### headWorldPos = self.worldCamera.position - targetPos = self.worldCamera.targetPosition - forwardVec, upVec, lateralVec = self.worldCamera.getVectors() + targetPos = self.worldCamera.target_position + forwardVec, upVec, lateralVec = self.worldCamera.get_vectors() if self.prevOculusHeadPos is None: self.prevOculusHeadPos = self.head[0] @@ -174,15 +174,15 @@ class OculusInterface(VRInterface): #targetPos += (self.head[0] - self.prevOculusHeadPos) lateral, forward = self.leftJoystick[-1] # move the camera by translating targetPos += 0.1 * (forward * forwardVec + lateral * lateralVec) - self.worldCamera.targetPosition = targetPos + self.worldCamera.target_position = targetPos # update hand positions in world leftHandWorldPos = headWorldPos + (self.leftHand[0] - self.head[0]) rightHandWorldPos = headWorldPos + (self.rightHand[0] - self.head[0]) - #self.world.moveObject(self.leftSphere, self.leftHand[0], (0, 0, 0, 1)) - #self.world.moveObject(self.rightSphere, self.rightHand[0], (0, 0, 0, 1)) - self.world.moveObject(self.leftSphere, leftHandWorldPos, (0, 0, 0, 1)) - self.world.moveObject(self.rightSphere, rightHandWorldPos, (0, 0, 0, 1)) + #self.world.move_object(self.leftSphere, self.leftHand[0], (0, 0, 0, 1)) + #self.world.move_object(self.rightSphere, self.rightHand[0], (0, 0, 0, 1)) + self.world.move_object(self.leftSphere, leftHandWorldPos, (0, 0, 0, 1)) + self.world.move_object(self.rightSphere, rightHandWorldPos, (0, 0, 0, 1)) # change color if hands collide with an object self.leftCollided = self.updateSphereColor(self.leftSphere, self.leftCollided, @@ -207,9 +207,9 @@ class OculusInterface(VRInterface): self.cnt += 1 def updateSphereColor(self, sphere, hasCollidedPreviously, collisionColor, freeColor): - aabb = self.world.getObjectAABB(sphere) + aabb = self.world.get_object_aabb(sphere) - if len(self.world.getObjectIdsInAABB(aabb[0], aabb[1])) > 1: + if len(self.world.get_object_ids_in_aabb(aabb[0], aabb[1])) > 1: update = not hasCollidedPreviously else: update = hasCollidedPreviously @@ -217,9 +217,9 @@ class OculusInterface(VRInterface): if update: hasCollidedPreviously = not hasCollidedPreviously if hasCollidedPreviously: - self.world.changeObjectColor(sphere, color=collisionColor) + self.world.change_object_color(sphere, color=collisionColor) else: - self.world.changeObjectColor(sphere, color=freeColor) + self.world.change_object_color(sphere, color=freeColor) return hasCollidedPreviously @@ -248,7 +248,7 @@ class OculusInterface(VRInterface): eyeTargetPos = targetPos + beta * lateralVec V = self.sim.computeViewMatrix(cameraEyePosition=eyePos, cameraTargetPosition=eyeTargetPos, cameraUpVector=(0,0,1)) - pic = np.array(self.sim.getCameraImage(self.width, self.height, viewMatrix=V)[2]) + pic = np.array(self.sim.get_camera_image(self.width, self.height, viewMatrix=V)[2]) pic = pic.reshape(self.width, self.height, 4)[:, :, :3] return pic diff --git a/pyrobolearn/utils/__init__.py b/pyrobolearn/utils/__init__.py index 5efc039..603ea65 100644 --- a/pyrobolearn/utils/__init__.py +++ b/pyrobolearn/utils/__init__.py @@ -3,13 +3,15 @@ import inspect import types import numpy as np + # Built-in functions -def hasAttribute(object, name): +def has_attribute(object, name): """Check if the given object has an attribute (variable or method) with the given name""" return hasattr(object, name) -def hasVariable(object, name): + +def has_variable(object, name): """Check if the given object has a variable with the given name""" attribute = getattr(object, name, None) if attribute is not None: @@ -20,70 +22,86 @@ def hasVariable(object, name): return not isinstance(attribute, types.MethodType) # types.FunctionType return False -def hasMethod(object, name): + +def has_method(object, name): """Check if the given object has a method with the given name""" method = getattr(object, name, None) return inspect.ismethod(method) -def isMethod(object): + +def is_method(object): """Check if the given object is a method""" return inspect.ismethod(object) -def isClass(object): + +def is_class(object): """Check if the given object is a class""" return inspect.isclass(object) -def isModule(object): + +def is_module(object): """Check if the given object is a module""" return inspect.ismodule(object) -def isList(object): + +def is_list(object): """Check if the given object is a list""" return isinstance(object, list) -def isTuple(object): + +def is_tuple(object): """Check if the given object is a tuple""" return isinstance(object, tuple) -def isNumpyArray(object): + +def is_numpy_array(object): """Check if the given object is a numpy array""" return isinstance(object, np.ndarray) -def isDict(object): + +def is_dict(object): """Check if the given object is a dictionary""" return isinstance(object, dict) -def isSet(object): + +def is_set(object): """Check the given object is a set""" return isinstance(object, set) -def isNone(object): + +def is_none(object): """Check if the given object is None""" return object is None -def isInt(object): + +def is_int(object): """Check if the given object is an integer""" return isinstance(object, int) -def isFloat(object): + +def is_float(object): """Check if the given object is a float""" return isinstance(object, float) -def isStr(object): + +def is_str(object): """Check if the given object is a string""" return isinstance(object, str) -def isChar(object): + +def is_char(object): """Check if the given object is a character""" if isinstance(object, str): if len(object) == 1: return True return False -def isBool(object): + +def is_bool(object): """Check if the given object is a boolean""" return isinstance(object, bool) -def isComplex(object): + +def is_complex(object): """Check if the given object is a complex number""" - return isinstance(object, complex) \ No newline at end of file + return isinstance(object, complex) diff --git a/pyrobolearn/utils/orientation.py b/pyrobolearn/utils/orientation.py index 5d40286..08ea087 100644 --- a/pyrobolearn/utils/orientation.py +++ b/pyrobolearn/utils/orientation.py @@ -568,20 +568,32 @@ def get_quaternion_product(q1, q2, convention='xyzw'): if isinstance(q1, quaternion.quaternion): return q1*q2 elif isinstance(q1, Iterable): - if convention == 'xyzw': - x1, y1, z1, w1 = q1 - x2, y2, z2, w2 = q2 - v1, v2 = np.array([x1, y1, z1]), np.array([x2, y2, z2]) - v = w1 * v2 + w2 * v1 + np.cross(v1, v2) - w = w1 * w2 - v1.dot(v2) - return np.array([v[0], v[1], v[2], w]) - elif convention == 'wxyz': - w1, x1, y1, z1 = q1 - w2, x2, y2, z2 = q2 - v1, v2 = np.array([x1, y1, z1]), np.array([x2, y2, z2]) - v = w1 * v2 + w2 * v1 + np.cross(v1, v2) - w = w1 * w2 - v1.dot(v2) - return np.array([w, v[0], v[1], v[2]]) + + def product(q1, q2, convention): + if convention == 'xyzw': + x1, y1, z1, w1 = q1 + x2, y2, z2, w2 = q2 + v1, v2 = np.array([x1, y1, z1]), np.array([x2, y2, z2]) + v = w1 * v2 + w2 * v1 + np.cross(v1, v2) + w = w1 * w2 - v1.dot(v2) + return np.array([v[0], v[1], v[2], w]) + elif convention == 'wxyz': + w1, x1, y1, z1 = q1 + w2, x2, y2, z2 = q2 + v1, v2 = np.array([x1, y1, z1]), np.array([x2, y2, z2]) + v = w1 * v2 + w2 * v1 + np.cross(v1, v2) + w = w1 * w2 - v1.dot(v2) + return np.array([w, v[0], v[1], v[2]]) + + if isinstance(q1, np.ndarray): + if len(q1.shape) == 1 and len(q2.shape) == 1: + return product(q1, q2, convention) + elif len(q1.shape) == 2 and len(q2.shape) == 1: + return np.array([product(q, q2, convention) for q in q1]) + elif len(q1.shape) == 1 and len(q2.shape) == 2: + return np.array([product(q1, q, convention) for q in q2]) + else: + return np.array([product(q1_, q2_, convention) for q1_, q2_ in zip(q1, q2)]) else: raise NotImplementedError("Asking for a convention that has not been implemented") else: @@ -657,3 +669,9 @@ if __name__ == "__main__": print('Quaternion <- matrix <- RPY: {}'.format(get_quaternion_from_matrix(get_matrix_from_rpy(rpy)))) print('Using pybullet: {}'.format(pybullet.getQuaternionFromEuler(rpy))) print('Using tf.transformations: {}'.format(tft.quaternion_from_euler(*rpy))) + + import quaternion + q1 = quaternion.quaternion(q[3], q[0], q[1], q[2]) + q2 = q1 + print(q1 * q2) + print(get_quaternion_product(q, q, convention='xyzw')) diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index 6387fee..01245dd 100644 --- a/pyrobolearn/worlds/world.py +++ b/pyrobolearn/worlds/world.py @@ -7,6 +7,7 @@ Dependencies: """ import collections +import inspect import multiprocessing import os import numpy as np @@ -14,9 +15,11 @@ import numpy as np import cv2 import time +from pyrobolearn.simulators import Simulator + from pyrobolearn.utils.converter import QuaternionListConverter # from pyrobolearn.utils.heightmap_generator import * # TODO: problem with gdal installation -from pyrobolearn.utils import hasMethod, hasVariable, isClass +from pyrobolearn.utils import has_method, has_variable from pyrobolearn.robots import Robot, robot_names_to_classes # from pyrobolearn.tools.bridges.bridge import Bridge @@ -71,10 +74,10 @@ class WorldCamera(object): """ Return all the information about the camera. """ - return self.getDebugVisualizerCamera(convert=False) + return self.get_debug_visualizer_camera(convert=False) # alias - def getDebugVisualizerCamera(self, convert=True): + def get_debug_visualizer_camera(self, convert=True): """ Return all the information provided by the camera. @@ -84,9 +87,9 @@ class WorldCamera(object): Returns: width (int): width of the camera image in pixels height (int): height of the camera image in pixels - viewMatrix (float[16], float[4x4]): view matrix of the camera - projectionMatrix (float[16], float[4x4]): projection matrix of the camera - cameraUp (float[3]): up axis of the camera, in Cartesian world space coordinates + view_matrix (float[16], float[4x4]): view matrix of the camera + projection_matrix (float[16], float[4x4]): projection matrix of the camera + camera_up (float[3]): up axis of the camera, in Cartesian world space coordinates cameraForward (float[3]): forward axis of the camera, in Cartesian world space coordinates horizontal (float[3]): TBD. This is a horizontal vector that can be used to generate rays (for mouse picking or creating a simple ray tracer for example) @@ -98,49 +101,31 @@ class WorldCamera(object): target (float[3]): target of the camera, in Cartesian world space coordinates """ - if convert: - width, height, viewMatrix, projectionMatrix, cameraUp,\ - cameraForward, horizontal, vertical, yaw, pitch, dist, target = self.sim.getDebugVisualizerCamera() - - # convert - viewMatrix = np.array(viewMatrix).reshape(4, 4).T - projectionMatrix = np.array(projectionMatrix).reshape(4, 4).T - cameraUp = np.array(cameraUp) - cameraForward = np.array(cameraForward) - horizontal = np.array(horizontal) - vertical = np.array(vertical) - target = np.array(target) - yaw, pitch = np.deg2rad(yaw), np.deg2rad(pitch) - - # return - return width, height, viewMatrix, projectionMatrix, cameraForward, cameraForward, horizontal, vertical,\ - yaw, pitch, dist, target - return self.sim.getDebugVisualizerCamera() + return self.sim.get_debug_visualizer() @property def width(self): """ Return the width of the pictures (in pixel) """ - return self.sim.getDebugVisualizerCamera()[0] + return self.sim.get_debug_visualizer()[0] @property def height(self): """ Return the height of the pictures (in pixel) """ - return self.sim.getDebugVisualizerCamera()[1] + return self.sim.get_debug_visualizer()[1] @property def V(self): """ Return the view matrix, which maps from the world to the view space. """ - viewMatrix = self.sim.getDebugVisualizerCamera()[2] - return np.array(viewMatrix).reshape(4, 4).T + return self.sim.get_debug_visualizer()[2] # alias - viewMatrix = V + view_matrix = V @property def Vinv(self): @@ -154,11 +139,10 @@ class WorldCamera(object): """ Return the projection matrix, which maps from the view to the projected/clipped space. """ - projectionMatrix = self.sim.getDebugVisualizerCamera()[3] - return np.array(projectionMatrix).reshape(4, 4).T + return self.sim.get_debug_visualizer()[3] # alias - projectionMatrix = P + projection_matrix = P @property def Pinv(self): @@ -172,13 +156,13 @@ class WorldCamera(object): """ Return the viewport matrix, which maps from the normalized clip coordinates to pixel coordinates. """ - width, height = self.sim.getDebugVisualizerCamera()[:2] + width, height = self.sim.get_debug_visualizer()[:2] return np.array([[width / 2, 0, 0, width / 2], [0, height / 2, 0, height / 2], [0, 0, 0.5, 0.5], [0, 0, 0, 1]]) - viewportMatrix = Vp + viewport_matrix = Vp @property def Vp_inv(self): @@ -187,13 +171,11 @@ class WorldCamera(object): """ return np.linalg.inv(self.Vp) - def getMatrices(self, inverse=False): + def get_matrices(self, inverse=False): """ Return the view, projection, and viewport matrices. """ - width, height, V, P = self.sim.getDebugVisualizerCamera()[:4] - V = np.array(V).reshape(4, 4).T - P = np.array(P).reshape(4, 4).T + width, height, V, P = self.sim.get_debug_visualizer()[:4] Vp = np.array([[width / 2, 0, 0, width / 2], [0, height / 2, 0, height / 2], [0, 0, 0.5, 0.5], @@ -206,134 +188,133 @@ class WorldCamera(object): return V, P, Vp @property - def upVector(self): + def up_vector(self): """ Return the up axis of the camera in the Cartesian world space coordinates """ - return np.array(self.sim.getDebugVisualizerCamera()[4]) + return self.sim.get_debug_visualizer()[4] @property - def forwardVector(self): + def forward_vector(self): """ Return the forward axis of the camera in the Cartesian world space coordinates. """ - return np.array(self.sim.getDebugVisualizerCamera()[5]) + return self.sim.get_debug_visualizer()[5] - def getVectors(self): + def get_vectors(self): """ Return the forward, up, and lateral vectors of the camera. """ - upVector, forwardVector = self.sim.getDebugVisualizerCamera()[4:6] - upVector, forwardVector = np.array(upVector), np.array(forwardVector) - lateralVector = np.cross(forwardVector, upVector) - return forwardVector, upVector, lateralVector + up_vector, forward_vector = self.sim.get_debug_visualizer()[4:6] + lateral_vector = np.cross(forward_vector, up_vector) + return forward_vector, up_vector, lateral_vector @property def yaw(self): """ Return the yaw angle of the camera in radian """ - return np.deg2rad(self.sim.getDebugVisualizerCamera()[8]) + return self.sim.get_debug_visualizer()[8] @property def pitch(self): """ Return the pitch angle of the camera. """ - return np.deg2rad(self.sim.getDebugVisualizerCamera()[9]) + return self.sim.get_debug_visualizer()[9] @property def dist(self): """ Return the distance between the camera and the camera target. """ - return self.sim.getDebugVisualizerCamera()[10] + return self.sim.get_debug_visualizer()[10] @property - def targetPosition(self): + def target_position(self): """ Return the target of the camera in the Cartesian world space coordinates. """ - return np.array(self.sim.getDebugVisualizerCamera()[11]) + return self.sim.get_debug_visualizer()[11] - @targetPosition.setter - def targetPosition(self, pos): - yaw, pitch, dist = self.sim.getDebugVisualizerCamera()[-4:-1] - self.sim.resetDebugVisualizerCamera(dist, yaw, pitch, pos) + @target_position.setter + def target_position(self, pos): + yaw, pitch, dist = self.sim.get_debug_visualizer()[-4:-1] + self.sim.reset_debug_visualizer(dist, yaw, pitch, pos) @property def position(self): """ Return the current position of the camera in the Cartesian world space coordinates. """ - Vinv = np.linalg.inv(self.V) # compute inverse of the view matrix - position = Vinv[:3,3] # the last column is the current position of the camera + Vinv = np.linalg.inv(self.V) # compute inverse of the view matrix + position = Vinv[:3, 3] # the last column is the current position of the camera return position @position.setter def position(self, pos): - self.sim.resetDebugVisualizerCamera(dist, yaw, pitch, targetPos) + self.sim.reset_debug_visualizer(dist, yaw, pitch, targetPos) @property def orientation(self): - # based on forwardVector and upVector + # based on forward_vector and up_vector pass @orientation.setter def orientation(self, orientation): pass - def setYawPitch(self, yaw, pitch, radian=True): + def set_yaw_pitch(self, yaw, pitch, radian=True): if radian: yaw, pitch = np.rad2deg(yaw), np.rad2deg(pitch) - dist, targetPos = self.sim.getDebugVisualizerCamera()[-2:] - self.sim.resetDebugVisualizerCamera(dist, yaw, pitch, targetPos) + dist, target_pos = self.sim.get_debug_visualizer()[-2:] + self.sim.reset_debug_visualizer(dist, yaw, pitch, target_pos) - def addYawPitch(self, dyaw, dpitch, radian=True): - yaw, pitch, dist, targetPos = self.sim.getDebugVisualizerCamera()[-4:] + def add_yaw_pitch(self, dyaw, dpitch, radian=True): + yaw, pitch, dist, target_pos = self.sim.get_debug_visualizer()[-4:] if radian: dyaw, dpitch = np.rad2deg(dyaw), np.rad2deg(dpitch) yaw += dyaw pitch += dpitch - self.sim.resetDebugVisualizerCamera(dist, yaw, pitch, targetPos) + self.sim.reset_debug_visualizer(dist, yaw, pitch, target_pos) - def getRGBImage(self): + def get_rgb_image(self): """ Return the captured RGB image. """ - return self.getRGBAImage()[:, :, :3] + return self.get_rgba_image()[:, :, :3] - def getRGBAImage(self): + def get_rgba_image(self): """ Return the captured RGBA image. 'A' stands for alpha channel (for opacity/transparency) """ - width, height, viewMatrix, projectionMatrix = self.sim.getDebugVisualizerCamera()[:4] - img = np.array(self.sim.getCameraImage(width, height, viewMatrix, projectionMatrix)[2]) + width, height, view_matrix, projection_matrix = self.sim.get_debug_visualizer()[:4] + img = np.array(self.sim.get_camera_image(width, height, view_matrix, projection_matrix)[2]) img = img.reshape(width, height, 4) # RGBA return img - def getDepthImage(self): + def get_depth_image(self): """ Return the depth image. """ - width, height, viewMatrix, projectionMatrix = self.sim.getDebugVisualizerCamera()[:4] - img = np.array(self.sim.getCameraImage(width, height, viewMatrix, projectionMatrix)[3]) + width, height, viewMatrix, projectionMatrix = self.sim.get_debug_visualizer()[:4] + img = np.array(self.sim.get_camera_image(width, height, viewMatrix, projectionMatrix)[3]) img = img.reshape(width, height) return img - def getRGBADImage(self, concatenate=True): + def get_rgbad_image(self, concatenate=True): """ Return the RGBA and depth images. """ - width, height, viewMatrix, projectionMatrix = self.sim.getDebugVisualizerCamera()[:4] - rgba, depth = self.sim.getCameraImage(width, height, viewMatrix, projectionMatrix)[2:4] + width, height, view_matrix, projection_matrix = self.sim.get_debug_visualizer()[:4] + rgba, depth = self.sim.get_camera_image(width, height, view_matrix, projection_matrix)[2:4] rgba = np.array(rgba).reshape(width, height, 4) depth = np.array(depth).reshape(width, height) if concatenate: return np.dstack((rgba, depth)) return (rgba, depth) - def screenToWorld(self, x_screen, Vp_inv=None, P_inv=None, V_inv=None): + def screen_to_world(self, x_screen, Vp_inv=None, P_inv=None, V_inv=None): """ Return the corresponding coordinates in the Cartesian world space from the coordinates of a point on the screen. @@ -347,9 +328,12 @@ class WorldCamera(object): Returns: float[4]: augmented vector coordinates of the corresponding point in the world """ - if Vp_inv is None: Vp_inv = self.Vp_inv - if P_inv is None: P_inv = self.Pinv - if V_inv is None: V_inv = self.Vinv + if Vp_inv is None: + Vp_inv = self.Vp_inv + if P_inv is None: + P_inv = self.Pinv + if V_inv is None: + V_inv = self.Vinv x_ndc = Vp_inv.dot(x_screen) x_ndc[1] = -x_ndc[1] # invert y-axis @@ -359,7 +343,7 @@ class WorldCamera(object): x_world = V_inv.dot(x_eye) return x_world - def worldToScreen(self, x_world, V=None, P=None, Vp=None): + def world_to_screen(self, x_world, V=None, P=None, Vp=None): """ Return the corresponding screen coordinates from a 3D point in the world. @@ -410,13 +394,16 @@ class World(object): """ def __init__(self, simulator, gravity=(0., 0., -9.81)): - self.sim = simulator - # self.sim.setAdditionalSearchPath(pybullet_data.getDataPath()) + # set simulator + self.simulator = simulator + + # By default, set the gravity + self.gravity = gravity self.robots = {} - self.movable_objects = {} # set() - self.immovable_objects = {} # set() - self.visual_objects = {} # set() + self.movable_bodies = {} # set() + self.immovable_bodies = {} # set() + self.visual_objects = {} # set() self.visual_shapes = {} self.map = None @@ -424,13 +411,10 @@ class World(object): self.quaternion_converter = QuaternionListConverter(convention=1) - # By default, set the gravity - self.setGravity(gravity) - - self.worldState = None + self.world_state = None # configure debug visualizer - # self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_GUI, 0) + self.sim.configure_debug_visualizer(self.sim.COV_ENABLE_GUI, 0) # interfaces and bridges self.interfaces = set([]) @@ -442,11 +426,136 @@ class World(object): @property def simulator(self): + """Return the simulator instance.""" return self.sim + @simulator.setter + def simulator(self, simulator): + """Set the simulator instance.""" + if not isinstance(simulator, Simulator): + raise TypeError("Expecting the given simulator to be an instance of `Simulator`, instead got: " + "{}".format(type(simulator))) + self.sim = simulator + @property - def mainCamera(self): - return self.getMainCamera() + def main_camera(self): + """Return the main camera of the simulator.""" + return WorldCamera(self.sim) + + @property + def gravity(self): + """Return the gravity vector.""" + return self._gravity + + @gravity.setter + def gravity(self, gravity): + """Set the gravity vector in the world. + + Args: + gravity (np.float[3]): 3d gravity vector. + """ + gravity = np.array(gravity) + self.sim.set_gravity(gravity) + self._gravity = gravity + + @property + def lateral_friction(self): + """Return the floor lateral friction coefficient.""" + if self.floor_id > 0: + return self.sim.get_dynamics_info(self.floor_id, link_id=-1)[1] + + @lateral_friction.setter + def lateral_friction(self, coefficient): + """Set the floor lateral friction coefficient.""" + if self.floor_id > 0: + self.sim.change_dynamics(body_id=self.floor_id, link_id=-1, lateral_friction=coefficient) + + @property + def rolling_friction(self): + """Return the floor rolling friction coefficient.""" + if self.floor_id > 0: + return self.sim.get_dynamics_info(self.floor_id, -1)[6] + + @rolling_friction.setter + def rolling_friction(self, coefficient): + """Set the floor rolling friction coefficient.""" + if self.floor_id > 0: + self.sim.change_dynamics(body_id=self.floor_id, link_id=-1, rolling_friction=coefficient) + + @property + def spinning_friction(self): + """Return the floor spinning friction coefficient.""" + if self.floor_id > 0: + return self.sim.get_dynamics_info(self.floor_id, -1)[7] + + @spinning_friction.setter + def spinning_friction(self, coefficient): + """Set the spinning friction coefficient.""" + if self.floor_id > 0: + self.sim.change_dynamics(body_id=self.floor_id, link_id=-1, spinning_friction=coefficient) + + @property + def restitution(self): + """Return the floor restitution (bounciness) coefficient.""" + if self.floor_id > 0: + return self.sim.get_dynamics_info(self.floor_id, -1)[5] + + @restitution.setter + def restitution(self, coefficient): + """Set the floor restitution (bounciness) coefficient.""" + if self.floor_id > 0: + self.sim.change_dynamics(body_id=self.floor_id, link_id=-1, restitution=coefficient) + + @property + def contact_damping(self): + """Return the floor contact damping.""" + if self.floor_id > 0: + return self.sim.get_dynamics_info(self.floor_id, -1)[8] + + @contact_damping.setter + def contact_damping(self, value): + """Set the floor contact damping value.""" + if self.floor_id > 0: + self.sim.change_dynamics(body_id=self.floor_id, link_id=-1, contact_damping=value) + + @property + def contact_stiffness(self): + """Return the floor contact stiffness.""" + if self.floor_id > 0: + return self.sim.get_dynamics_info(self.floor_id, -1)[9] + + @contact_stiffness.setter + def contact_stiffness(self, value): + """Set the floor contact stiffness value.""" + if self.floor_id > 0: + self.sim.change_dynamics(body_id=self.floor_id, link_id=-1, contact_stiffness=value) + + @property + def floor_dynamics(self): + """Return the floor dynamical parameters (friction, restitution, etc). + + Returns: + float: lateral friction coefficient + float: rolling friction coefficient + float: spinning friction coefficient + float: restitution coefficient + float: contact damping value + float: contact stiffness value + """ + if self.floor_id > 0: + info = self.sim.get_dynamics_info(self.floor_id, -1) + return info[1], info[6], info[7], info[5], info[8], info[9] + + @floor_dynamics.setter + def floor_dynamics(self, dynamics): + """ + Set the floor dynamics. + + Args: + dynamics (dict): dictionary of coefficients. + """ + if self.floor_id > 0: + self.sim.change_dynamics(body_id=self.floor_id, link_id=-1, **dynamics) ######################## # Operator Overloading # @@ -468,14 +577,14 @@ class World(object): if not isinstance(item, int): item = item.id - return (item in self.robots) or (item in self.movable_objects) or (item in self.immovable_objects) \ + return (item in self.robots) or (item in self.movable_bodies) or (item in self.immovable_bodies) \ or (item in self.visual_objects) ########### # Methods # ########### - def setBridges(self, bridges): + def set_bridges(self, bridges): """ This append the given bridges to various interfaces to the list of bridges. @@ -488,92 +597,70 @@ class World(object): for bridge in bridges: # if not isinstance(bridge, Bridge): # raise TypeError("Expecting a list of bridges (must be an instance of Bridge)") - if not hasMethod(bridge, 'step') and not hasVariable(bridge, 'interface'): + if not has_method(bridge, 'step') and not has_variable(bridge, 'interface'): raise TypeError("Expecting bridge to have a `step` method and an `interface` variable") - if not hasMethod(bridge.interface, 'step'): + if not has_method(bridge.interface, 'step'): raise TypeError("Expecting the bridge.interface to have a `step` method") self.bridges.append(bridge) self.interfaces.add(bridge.interface) # elif isinstance(bridges, Bridge): - elif hasMethod(bridges, 'step') and hasVariable(bridges, 'interface') and hasMethod(bridges.interface, 'step'): + elif has_method(bridges, 'step') and has_variable(bridges, 'interface') and has_method(bridges.interface, 'step'): self.bridges.append(bridges) self.interfaces.add(bridges.interface) else: raise TypeError("Expecting a bridge (instance of Bridge) or a list of instances of Bridge") - def setRealTimeSimulation(self, enable=True): - """ - Simulate in real-time. This is wrapper around the original `setRealTimeSimulation` method. - From the user-guide (https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA): - "By default, the physics server will not step the simulation, unless you explicitly send a 'stepSimulation' - command. This way you can maintain control determinism of the simulation. It is possible to run the - simulation in real-time by letting the physics server automatically step the simulation according to - its real-time-clock (RTC) using the setRealTimeSimulation command. If you enable the real-time simulation, - you don't need to call 'stepSimulation'. - - Note that setRealTimeSimulation has no effect in DIRECT mode: in DIRECT mode the physics server and client - happen in the same thread and you trigger every command. In GUI mode and in Virtual Reality mode, and TCP/UDP - mode, the physics server runs in a separate thread from the client (PyBullet), and setRealTimeSimulation - allows the physicsserver thread to add additional calls to stepSimulation." - - Args: - enable (bool): If True, it enables the real-time simulation. If False, it disables it. - """ - self.sim.setRealTimeSimulation(enable) - def save(self, filename=None): """ - Save the world in the given filename. If the filename is None, it will save it in memory (RAM). + Save the world in the given filename, or in the RAM. + + Args: + filename (str, None): path to file to save the state of the world. If None, it will save it in the main + memory (RAM). + + Returns: + str or int: filename, or unique state id. """ # save approximate world state on the disk # self.sim.saveWorld(filename) + self.world_state = self.sim.save(filename) + return self.world_state - if filename is None: - # save world state in memory (RAM) - self.worldState = self.sim.saveState() - else: - # save world state on the disk - self.sim.saveBullet(filename) - self.worldState = filename - return self.worldState - - def reset(self): + def reset(self, world_state=None): """ Reset the world. Put back each object where they were when added to the world. - """ - # save to current instance if not already saved - if self.worldState is None: - self.worldState = self.save() - # reset world to a previous instance - if isinstance(self.worldState, int): - # load world state from memory - self.sim.restoreState(self.worldState) - # reset the robot - # for robot in self.robots.values(): - # # robot.resetBasePositionAndOrientation() - # # robot.resetBaseVelocity() - # robot.resetJointStates() - # robot.setJointInitPositions() - elif isinstance(self.worldState, str): - # load world state from the disk - self.sim.restoreState(fileName=self.worldState) + Args: + world_state (int, str, None): world state id (int), or path to file (str). If None, it will restore the + last saved world state. + """ + if world_state is None: + # save to current instance if not already saved + if self.world_state is None: + self.world_state = self.save() + + # reset world to a previous instance + if isinstance(self.world_state, (int, str)): + # load world from the disk / memory + self.sim.load(self.world_state) + # reset the robot + # self.reset_robots() + else: + # reset simulation: remove all objects from the world and reset the world to initial conditions + self.sim.reset() else: - # reset simulation: remove all objects from the world and reset the world to initial conditions - self.sim.resetSimulation() + if isinstance(world_state, (int, float)): + # load world state from memory / disk + self.sim.load(world_state) + else: + raise TypeError("Expecting the world state to be an int (id) or a string (path to file), instead got " + "{}".format(type(world_state))) - # print("\nRobot reset state:") - # for robot_id, robot in self.robots.items(): - # print("Robot base position and orientation: {}".format(robot.getBasePositionAndOrientation())) - # print("Robot base velocities: {}".format(robot.getBaseVelocity())) - # print("Robot joint positions: {}".format(robot.getJointPositions())) - # print("Robot joint velocities: {}".format(robot.getJointVelocities())) - - def resetSimulator(self): + def reset_simulator(self): """ - Reset the simulator. + Reset the simulator; remove the world from the simulator. """ - self.sim.resetSimulation() + self.sim.reset() def step(self, sleep_dt=None): """ @@ -583,27 +670,11 @@ class World(object): interface.step() for bridge in self.bridges: bridge.step() - self.sim.stepSimulation() + self.sim.step() if sleep_dt is not None: time.sleep(sleep_dt) - def setGravity(self, xyz=(0., 0., -9.81)): - """ - Set the given gravity. - - Args: - xyz (float[3]): gravity (acceleration) along the 3 axis. - """ - x, y, z = xyz - self.sim.setGravity(x, y, z) - - def getMainCamera(self): - """ - Return the main camera of the simulator. - """ - return WorldCamera(self.sim) - - def loadRobot(self, robot, position=None, orientation=None, useFixedBase=None, *args, **kwargs): + def load_robot(self, robot, position=None, orientation=None, fixed_base=None, *args, **kwargs): """ Load the robot into the world. If the robot parameter is a known robot name or the path to the urdf file, it will create a `Robot` instance and return it. If the robot is already an instance of `Robot` it will @@ -614,7 +685,7 @@ class World(object): `implemented_robots` from `pyrobolearn.robots` module. position (float[3], None): position of the robot. If None, it will take the default position. orientation (float[4], None): orientation of the robot. If None, it will be the default orientation. - useFixedBase (bool, None): if True, it will fix the robot's base. If None, it will be the default option. + fixed_base (bool, None): if True, it will fix the robot's base. If None, it will be the default option. Return: Robot: instance of the Robot class @@ -627,16 +698,15 @@ class World(object): if robot in robot_names_to_classes: robot_class = robot_names_to_classes[robot] - robot = robot_class(self.sim, init_pos=position, init_orient=orientation, useFixedBase=useFixedBase, + robot = robot_class(self.sim, position=position, orientation=orientation, fixed_base=fixed_base, *args, **kwargs) else: # robot is the path to the urdf - robot = Robot(self.sim, urdf_path=robot, init_pos=position, init_orient=orientation, - useFixedBase=useFixedBase, *args, **kwargs) + robot = Robot(self.sim, urdf=robot, position=position, orientation=orientation, fixed_base=fixed_base, + *args, **kwargs) - elif isClass(robot): # robot class - robot = robot(self.sim, init_pos=position, init_orient=orientation, useFixedBase=useFixedBase, - *args, **kwargs) + elif inspect.isclass(robot): # robot class + robot = robot(self.sim, position=position, orientation=orientation, fixed_base=fixed_base, *args, **kwargs) else: # unknown type raise TypeError('Unknown type for robot: {}. It must be a string or ' @@ -645,7 +715,7 @@ class World(object): self.robots[robot.id] = robot return robot - def isRobotId(self, robot_id): + def is_robot_id(self, robot_id): """ Check if the given id is a robot id. @@ -657,12 +727,12 @@ class World(object): """ return robot_id in self.robots - def getRobot(self, robotId): + def get_robot(self, robot_id): """ Return the robot object (instance of Robot) associated to the given robot id. Args: - robotId (int): unique id of the robot + robot_id (int): unique id of the robot Raises: KeyError: if the given robot id is not in the world. @@ -670,43 +740,43 @@ class World(object): Returns: Robot: robot instance """ - return self.robots[robotId] + return self.robots[robot_id] - def resetRobots(self): + def reset_robots(self): """ Reset the base and joint states of each robot """ for robot_id, robot in self.robots.items(): # reset base - self.sim.resetBasePositionAndOrientation(robot_id, robot.init_position, robot.init_orientation) - self.sim.resetBaseVelocity(robot_id, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0]) + self.sim.reset_base_pose(robot_id, robot.init_position, robot.init_orientation) + self.sim.reset_base_velocity(robot_id, linear_velocity=[0, 0, 0], angular_velocity=[0, 0, 0]) # reset joint positions positions = robot.init_joint_positions velocities = np.zeros(len(positions)) for joint_id, position, velocity in zip(robot.joints, positions, velocities): - self.sim.resetJointState(robot_id, joint_id, position, velocity) + self.sim.reset_joint_state(robot_id, joint_id, position, velocity) - def loadURDF(self, filename, position, orientation, useFixedBase=False, scaling=1., objectName=None): + def load_urdf(self, filename, position, orientation, fixed_base=False, scale=1., name=None): """ - Load URDF specified by the given path. This is basically a wrapper around the simulator's `loadURDF` method. + Load URDF specified by the given path. This is basically a wrapper around the simulator's `load_urdf` method. Args: filename (str): path to the URDF file position (float[3]): position of the object described in the URDF orientation (float[4]): orientation represented as a quaternion - usedFixedBase (bool): if the base of the object should be fixed or not - scaling (float): scale factor for the object - objectName (str, None): name of the object. If None, it will extract it from the URDF. + fixed_base (bool): if the base of the object should be fixed or not + scale (float): scale factor for the object + name (str, None): name of the object. If None, it will extract it from the URDF. Returns: int: unique id of the loaded body. """ - obj = self.sim.loadURDF(filename, position, orientation, useFixedBase=useFixedBase, globalScaling=scaling) - self.movable_objects[obj] = self.sim.getBodyInfo(obj) if objectName is None else objectName - return obj + body = self.sim.load_urdf(filename, position, orientation, use_fixed_base=fixed_base, scale=scale) + self.movable_bodies[body] = self.sim.get_body_info(body) if name is None else name + return body - def loadSDF(self, filename, scaling=1.): + def load_sdf(self, filename, scaling=1.): """ Load the given SDF file; this will thus load all the object described in a SDF file. @@ -717,12 +787,12 @@ class World(object): Returns: list(int): list of ids """ - objects = self.sim.loadSDF(filename, globalScaling=scaling) - for obj in objects: - self.movable_objects[obj] = self.sim.getBodyInfo(obj) - return objects + bodies = self.sim.load_sdf(filename, scaling=scaling) + for body in bodies: + self.movable_bodies[body] = self.sim.get_body_info(body) + return bodies - def loadMJCF(self, filename, scaling=1.): + def load_mjcf(self, filename, scaling=1.): """ Load the given MJCF file; this will thus load all the object described in a MJCF file. @@ -733,31 +803,31 @@ class World(object): Returns: list(int): list of ids """ - objects = self.sim.loadMJCF(filename) #, globalScaling=scaling) - for obj in objects: - self.movable_objects[obj] = self.sim.getBodyInfo(obj) - return objects + bodies = self.sim.load_mjcf(filename, scaling=scaling) + for body in bodies: + self.movable_bodies[body] = self.sim.getBodyInfo(body) + return bodies def _loadSDForURDF(self, path, position, orientation, scaling, objectType=None): extension_name = path.split('.')[-1] if extension_name == 'urdf': - object_id = self.sim.loadURDF(path, position, orientation, globalScaling=scaling) - self.movable_objects[object_id] = 'urdf' if objectType is None else objectType + object_id = self.sim.load_urdf(path, position, orientation, scale=scaling) + self.movable_bodies[object_id] = 'urdf' if objectType is None else objectType elif extension_name == 'sdf': - object_id = self.sim.loadSDF(path, globalScaling=scaling) # list of ids + object_id = self.sim.loadSDF(path, scale=scaling) # list of ids for i in object_id: # assume for now that the objects are movable... - self.movable_objects[i] = 'sdf' if objectType is None else objectType + self.movable_bodies[i] = 'sdf' if objectType is None else objectType else: raise ValueError('Extension name of the file is not known; this method only accepts URDF/SDF files.') return object_id - def loadObject(self, objectType, path=None, position=(0,0,0), orientation=(0,0,0,1), scaling=1.): + def load_object(self, object_type, path=None, position=(0, 0, 0), orientation=(0, 0, 0, 1), scaling=1.): """ Load the specified object. This is a method that allows you to quickly load stuffs however it is less accurate than other methods in this class. Args: - objectType (str): type of the object (name, 'sphere', + object_type (str): type of the object (name, 'sphere', path: position: orientation: @@ -769,27 +839,27 @@ class World(object): # check if an object has already been loaded at that place. if path is not None: - objectId = self._loadSDForURDF(path, position, orientation, scaling=1., objectType=objectType) + object_id = self._loadSDForURDF(path, position, orientation, scaling=1., objectType=object_type) else: - if objectType == 'sphere': - objectId = self.loadSphere(position) - elif objectType == 'box': - objectId = self.loadBox(position, orientation) - elif objectType == 'cylinder': - objectId = self.loadCylinder(position, orientation) - elif objectType == 'capsule': - objectId = self.loadCapsule(position, orientation) + if object_type == 'sphere': + object_id = self.load_sphere(position) + elif object_type == 'box': + object_id = self.load_box(position, orientation) + elif object_type == 'cylinder': + object_id = self.load_cylinder(position, orientation) + elif object_type == 'capsule': + object_id = self.load_capsule(position, orientation) else: raise TypeError("Object type not known...") - return objectId + return object_id - def moveObject(self, objectId, position=None, orientation=None): + def move_object(self, object_id, position=None, orientation=None): """ Move the given object at the specified position and orientation. Args: - objectId (int): object id + object_id (int): object id position (float[3]): new position of the object. If None, it will keep the old position. orientation (float[4]): new orientation of the object. If None, it will keep the old orientation. @@ -797,12 +867,12 @@ class World(object): None """ if position is None: - position = self.sim.getBasePositionAndOrientation(objectId)[0] + position = self.sim.get_base_pose(object_id)[0] if orientation is None: - orientation = self.sim.getBasePositionAndOrientation(objectId)[1] - self.sim.resetBasePositionAndOrientation(objectId, position, orientation) + orientation = self.sim.get_base_pose(object_id)[1] + self.sim.reset_base_pose(object_id, position, orientation) - def applyForce(self, objectId, linkId=-1, force=(0.,0.,0.), position=None, frameFlag=2): + def apply_force(self, object_id, link_id=-1, force=(0., 0., 0.), position=None, frame=2): """ Apply the given force on the specified object or link of the object. @@ -811,183 +881,183 @@ class World(object): - this does not work when using `sim.setRealTimeSimulation(1)`. Args: - objectId (int): object id to apply the force on - linkId (int): link id to apply the force, if -1 it will apply the force on the base + object_id (int): object id to apply the force on + link_id (int): link id to apply the force, if -1 it will apply the force on the base force (float[3]): Cartesian forces to be applied on the body position (float[3]): position on the link where the force is applied. If None, it is the center of mass of the object (or the link if specified) - frameFlag (int): allows to specify the coordinate system of force/position. sim.LINK_FRAME (=1) for local + frame (int): allows to specify the coordinate system of force/position. sim.LINK_FRAME (=1) for local link frame, and sim.WORLD_FRAME (=2) for world frame. By default, it is the world frame. Returns: None """ if position is None: - if linkId != -1: - position = self.sim.getBasePositionAndOrientation(objectId)[0] + if link_id != -1: + position = self.sim.get_base_pose(object_id)[0] else: - position = self.sim.getLinkState(objectId, linkId)[0] - self.sim.applyExternalForce(objectId, linkId, force, position, frameFlag) + position = self.sim.get_link_state(object_id, link_id)[0] + self.sim.apply_external_force(object_id, link_id, force, position, frame) - def getObjectColor(self, objectId): + def get_object_color(self, object_id): """ Return the RGBA color of the given object. Args: - objectId (int): object id + object_id (int): object id Returns: float[4]: RGBA color """ - return self.sim.getVisualShapeData(objectId)[-1] + return self.sim.get_visual_shape_data(object_id)[-1] - def changeObjectColor(self, objectId, color=(1,1,1,1), linkId=-1): + def change_object_color(self, object_id, color, link_id=-1): """ Change the color of the given object. Args: - objectId (int): object id + object_id (int): object id color (float[4]): RGBA color - linkId (int): link id + link_id (int): link id Returns: None """ - self.sim.changeVisualShape(objectId, linkId, rgbaColor=color) + self.sim.change_visual_shape(object_id, link_id, rgba_color=color) - def getObjectPosition(self, objectId): + def get_object_position(self, object_id): """ Return the position of the given object. Args: - objectId (int): object id + object_id (int): object id Returns: float[3]: position of the object """ - return np.array(self.sim.getBasePositionAndOrientation(objectId)[0]) + return np.array(self.sim.get_base_pose(object_id)[0]) - def getObjectOrientation(self, objectId): + def get_object_orientation(self, object_id): """ Return the orientation of the given object. Args: - objectId (int): object id + object_id (int): object id Returns: float[4]: orientation of the object """ - return np.array(self.sim.getBasePositionAndOrientation(objectId)[1]) + return np.array(self.sim.get_base_pose(object_id)[1]) - def getObjectVelocity(self, objectId): + def get_object_velocity(self, object_id): """ Return the linear and angular velocities of the given object. Args: - objectId (int): object id + object_id (int): object id Returns: float[6]: linear and angular velocities of the object """ - lin_vel, ang_vel = self.sim.getBaseVelocity(objectId) + lin_vel, ang_vel = self.sim.get_base_velocity(object_id) return np.array(lin_vel + ang_vel) - def getObjectLinearVelocity(self, objectId): + def get_object_linear_velocity(self, object_id): """ Return the linear velocity of the given object. Args: - objectId (int): object id + object_id (int): object id Returns: float[3]: linear velocity of the object """ - return np.array(self.sim.getBaseVelocity(objectId)[0]) + return np.array(self.sim.get_base_velocity(object_id)[0]) - def getObjectAngularVelocity(self, objectId): + def get_object_angular_velocity(self, object_id): """ Return the angular velocity of the given object. Args: - objectId (int): object id + object_id (int): object id Returns: float[3]: angular velocity of the object """ - return np.array(self.sim.getBaseVelocity(objectId)[1]) + return np.array(self.sim.get_base_velocity(object_id)[1]) - def hideObject(self, objectId): + def hide_object(self, object_id): """ Hide (visually) the given object; by making it transparent. Args: - objectId (int): object id + object_id (int): object id Returns: None """ - color = self.getObjectColor(objectId) + color = self.get_object_color(object_id) color[-1] = 0. - self.changeObjectColor(objectId, color=color) + self.change_object_color(object_id, color=color) - def showObject(self, objectId): + def show_object(self, object_id): """ Show (visually) a hidden object; by making it opaque. Args: - objectId (int): object id + object_id (int): object id Returns: None """ - color = self.getObjectColor(objectId) + color = self.get_object_color(object_id) color[-1] = 1. - self.changeObjectColor(objectId, color=color) + self.change_object_color(object_id, color=color) - def removeObject(self, objectId): + def remove(self, body): """ Remove the object specified by its unique id from the world/simulator. Args: - objectId (int, Robot): unique id of the object in the simulator. + body (int, Robot): unique id of the object in the simulator. Returns: bool: True if succeeded, False if not. This method does not raise any errors. """ - if isinstance(objectId, Robot): - objectId = objectId.id - if objectId in self.robots: - self.robots.pop(objectId) - elif objectId in self.movable_objects: - self.movable_objects.pop(objectId) - elif objectId in self.immovable_objects: - self.immovable_objects.pop(objectId) - elif objectId in self.visual_objects: - self.visual_objects.pop(objectId) + if isinstance(body, Robot): + body = body.id + if body in self.robots: + self.robots.pop(body) + elif body in self.movable_bodies: + self.movable_bodies.pop(body) + elif body in self.immovable_bodies: + self.immovable_bodies.pop(body) + elif body in self.visual_objects: + self.visual_objects.pop(body) else: return False - self.sim.removeBody(objectId) + self.sim.remove_body(body) return True - def getObjectDimensions(self, objectId): + def get_object_dimensions(self, object_id): """ Return the object dimensions of the given object. Args: - objectId (int): object id + object_id (int): object id Returns: float[3]: dimensions of the object """ - return np.array(self.sim.getVisualShapeData[3]) + return np.array(self.sim.get_visual_shape_data(object_id)[3]) - def changeObjectScale(self, objectId, scale=(1.,1.,1.)): + def change_object_scale(self, object_id, scale=(1., 1., 1.)): """ Change the scale of the given object; it changes the scale for the visual and collision shapes. Args: - objectId (int): object id + object_id (int): object id scale (float[3]): scaling factors in each direction Returns: @@ -996,50 +1066,50 @@ class World(object): # TODO: currently not possible in PyBullet raise NotImplementedError - def getObjectAABB(self, objectId, linkId=-1): + def get_object_aabb(self, object_id, link_id=-1): """ Return the axis-aligned bounding box (AABB) in world space of the given object. Args: - objectId (int): object id - linkId (int): optional link id + object_id (int): object id + link_id (int): optional link id Returns: float[3]: coordinates in world space of the min corner of the AABB float[3]: coordinates in world space of the max corner of the AABB """ - bbMin, bbMax = self.sim.getAABB(objectId, linkId) - return np.array(bbMin), np.array(bbMax) + aabb_min, aabb_max = self.sim.get_aabb(object_id, link_id) + return np.array(aabb_min), np.array(aabb_max) - def getObjectIdsInAABB(self, bbMin, bbMax): + def get_object_ids_in_aabb(self, aabb_min, aabb_max): """ Get the list of object ids that have AABB overlap with a given AABB. Args: - bbMin (float[3]): coordinates of the min corner of the bounding box - bbMax (float[3]): coordinates of the max corner of the bounding box + aabb_min (float[3]): coordinates of the min corner of the bounding box + aabb_max (float[3]): coordinates of the max corner of the bounding box Returns: int[N]: list of object ids """ - overlapping_objects = self.sim.getOverlappingObjects(bbMin, bbMax) + overlapping_objects = self.sim.get_overlapping_objects(aabb_min, aabb_max) if overlapping_objects is None: return [] return overlapping_objects - def isThereAnObject(self, bbMin, bbMax, except_floor=True): + def is_there_an_object(self, aabb_min, aabb_max, except_floor=True): """ - Return True if there is an object in the bounding box defined by bbMin and bbMax. + Return True if there is an object in the bounding box defined by aabb_min and aabb_max. Args: - bbMin (float[3]): minimum coordinates of the bounding box - bbMax (float[3]): maximum coordinates of the bounding box + aabb_min (float[3]): minimum coordinates of the bounding box + aabb_max (float[3]): maximum coordinates of the bounding box except_floor (bool): if the floor should be counted as an object Returns: bool: True if there is an object in the specified bounding box """ - objects = self.sim.getOverlappingObjects(bbMin, bbMax) + objects = self.sim.get_overlapping_objects(aabb_min, aabb_max) if len(objects) > 2: return True if len(objects) == 0: @@ -1049,13 +1119,13 @@ class World(object): return False return True - def getClosestObject(self): # Not possible for now + def get_closest_object(self): # Not possible for now raise NotImplementedError - def getClosestObjects(self, radius): # Not possible for now + def get_closest_objects(self, radius): # Not possible for now raise NotImplementedError - def loadFloor(self, scaling=1.): + def load_floor(self, scaling=1.): """ Load a basic floor in the world. @@ -1065,11 +1135,11 @@ class World(object): Returns: int: unique id of the floor in the world """ - # self.floor_id = self.sim.loadURDF('plane100.urdf', useFixedBase=True, globalScaling=scaling) - self.floor_id = self.sim.loadURDF('plane.urdf', useFixedBase=True, globalScaling=scaling) + # self.floor_id = self.sim.load_urdf('plane100.urdf', use_fixed_base=True, scale=scaling) + self.floor_id = self.sim.load_urdf('plane.urdf', use_fixed_base=True, scale=scaling) return self.floor_id - def loadTerrain(self, heightmap, position=(0.,0.,0.), scaling=1., replace_floor=True): + def load_terrain(self, heightmap, position=(0., 0., 0.), scaling=1., replace_floor=True): """ Load the given terrain/heightmap. @@ -1092,18 +1162,18 @@ class World(object): """ if self.floor_id > -1: # there is already a floor defined if replace_floor: - self.sim.removeBody(self.floor_id) + self.sim.remove_body(self.floor_id) if heightmap[-4:] == 'obj': # obj (mesh) - self.floor_id = self.loadMesh(heightmap, position, mass=0., scale=[scaling]*3, - flags=self.sim.GEOM_FORCE_CONCAVE_TRIMESH, objectType='terrain') + self.floor_id = self.load_mesh(heightmap, position, mass=0., scale=[scaling] * 3, flags=1, + object_type='terrain') elif heightmap[-4:] == '.sdf': # SDF - self.floor_id = self.loadSDF(filename=heightmap, scaling=scaling) + self.floor_id = self.load_sdf(filename=heightmap, scaling=scaling) elif heightmap[-4:] == '.xml': # MJCF - self.floor_id = self.loadMJCF(filename=heightmap, scaling=scaling) + self.floor_id = self.load_mjcf(filename=heightmap, scaling=scaling) elif heightmap[-5:] == '.urdf': # URDF - self.floor_id = self.sim.loadURDF(heightmap, position, useFixedBase=True, globalScaling=scaling) - else: # heightmap (.tif, .jpg, .png, etc) + self.floor_id = self.sim.load_urdf(heightmap, position, use_fixed_base=True, scale=scaling) + else: # heightmap (.tif, .jpg, .png, etc) def create_mesh(heightmap): # create 3D mesh # create3DMesh(heightmap, filename=, subsample=, interpolate_fct=) @@ -1120,18 +1190,21 @@ class World(object): # apply the given texture if provided if isinstance(texture, str): - texture = self.sim.loadTexture(texture) - self.sim.changeVisualShape(heightmap, -1, textureUniqueId=texture) + texture = self.sim.load_texture(texture) + self.sim.change_visual_shape(heightmap, -1, texture_id=texture) return self.floor_id - def loadHeightmap(self, heightmap, texture=None, position=(0.,0.,0.), scale=1.): + def load_heightmap(self, heightmap, texture=None, position=(0., 0., 0.), scale=1.): """ Load a heightmap for the terrain. Args: heightmap (str, np.ndarray[M,M]): if string, filename containing the heightmap in the png, jpg, obj format if a 2D numpy arrays, the values represent the height in meters. + texture: texture to apply + position (float[3]): position of the terrain + scale (float): scaling factor Returns: int: unique id of the floor @@ -1165,7 +1238,7 @@ class World(object): raise ValueError("Expecting heightmap in a png/jpg/obj format") # load the mesh of the terrain - heightmap = self.loadTerrain(filename, position=position, scaling=scale) + heightmap = self.load_terrain(filename, position=position, scaling=scale) # change the dynamic properties of the terrain based on the given type (grass, mud, bumpy # WARNING: only 1 type can be specified. Currently, it is not possible to have different dynamic properties @@ -1174,8 +1247,8 @@ class World(object): # apply the given texture if provided if isinstance(texture, str): - texture = p.loadTexture(texture) - self.sim.changeVisualShape(heightmap, -1, textureUniqueId=texture) + texture = self.sim.load_texture(texture) + self.sim.change_visual_shape(heightmap, -1, texture_id=texture) # remove mesh from memory os.remove(filename + '.obj') # remove mesh from memory @@ -1183,13 +1256,13 @@ class World(object): # replace the floor if there is already one present if self.floor_id > -1: - self.sim.removeBody(self.floor_id) + self.sim.remove_body(self.floor_id) self.floor_id = heightmap return self.floor_id # aliases - loadDEM = loadHeightmap + loadDEM = load_heightmap def generateHeightmap(self, filename=None, algo=None): """ @@ -1210,10 +1283,10 @@ class World(object): """ pass - def generateTerrain(self, filename=None): + def generate_terrain(self, filename=None): pass - def loadStadium(self, scaling=1.): + def load_stadium(self, scaling=1.): """ Load a stadium as the floor. @@ -1224,11 +1297,11 @@ class World(object): int: unique id of the floor/stadium in the world """ if self.floor_id > -1: - self.sim.removeBody(self.floor_id) - self.floor_id = self.sim.loadURDF('stadium.urdf', useFixedBase=True, globalScaling=scaling) + self.sim.remove_body(self.floor_id) + self.floor_id = self.sim.load_urdf('stadium.urdf', use_fixed_base=True, scale=scaling) return self.floor_id - def loadJapaneseMonastery(self, scaling=1.): + def load_japanese_monastery(self, scaling=1.): """ Load a japanese monastery. @@ -1240,14 +1313,14 @@ class World(object): """ # replace the floor if there is already one present if self.floor_id > -1: - self.sim.removeBody(self.floor_id) - self.floor_id = self.sim.loadURDF('samurai.urdf', useFixedBase=True, globalScaling=scaling) + self.sim.remove_body(self.floor_id) + self.floor_id = self.sim.load_urdf('samurai.urdf', use_fixed_base=True, scale=scaling) return self.floor_id - def loadBotLab(self, scaling=2.): - return self.loadSDF('sdf/botlab/botlab.sdf', scaling=scaling) + def load_bot_lab(self, scaling=2.): + return self.load_sdf('sdf/botlab/botlab.sdf', scaling=scaling) - def loadStairs(self): + def load_stairs(self): pass def createCity(self): @@ -1270,11 +1343,11 @@ class World(object): Returns: int: unique id of the table """ - table = self.sim.loadURDF('table/table.urdf', globalScaling=scaling) - self.movable_objects[table] = 'table' + table = self.sim.load_urdf('table/table.urdf', scale=scaling) + self.movable_bodies[table] = 'table' return table - def loadKivaShelf(self, scaling=1.): + def load_kiva_shelf(self, scaling=1.): """ Load a Kiva shelf. @@ -1284,11 +1357,11 @@ class World(object): Returns: int: unique id of the shelf """ - shelf = self.sim.loadSDF('kiva_shelf/model.sdf', globalScaling=scaling)[0] - self.movable_objects[shelf] = 'shelf' + shelf = self.sim.loadSDF('kiva_shelf/model.sdf', scale=scaling)[0] + self.movable_bodies[shelf] = 'shelf' return shelf - def loadVisualSphere(self, position, radius=0.5, color=(1,1,1,1)): + def load_visual_Sphere(self, position, radius=0.5, color=None): """ Load a visual sphere in the world (only available in the simulator). @@ -1300,14 +1373,12 @@ class World(object): Returns: int: unique id of the visual sphere in the world """ - visualShape = self.sim.createVisualShape(self.sim.GEOM_SPHERE, radius=radius, rgbaColor = color) - sphere = self.sim.createMultiBody(baseMass = 0., - baseVisualShapeIndex = visualShape, - basePosition = position) + visual_shape = self.sim.create_visual_shape(self.sim.GEOM_SPHERE, radius=radius, rgba_color=color) + sphere = self.sim.create_body(visual_shape_id=visual_shape, mass=0., position=position) self.visual_objects[sphere] = 'sphere' return sphere - def loadSphere(self, position, mass=1., radius=0.5, color=(1,1,1,1)): + def load_sphere(self, position, mass=1., radius=0.5, color=None): """ Load a sphere in the world (only available in the simulator). @@ -1320,30 +1391,24 @@ class World(object): Returns: int: unique id of the sphere in the world """ - collisionShape = self.sim.createCollisionShape(self.sim.GEOM_SPHERE, - radius = radius) - visualShape = self.sim.createVisualShape(self.sim.GEOM_SPHERE, - radius = radius, - rgbaColor = color) - sphere = self.sim.createMultiBody(baseMass = mass, - baseCollisionShapeIndex = collisionShape, - baseVisualShapeIndex = visualShape, - basePosition = position) + collision_shape = self.sim.create_collision_shape(self.sim.GEOM_SPHERE, radius=radius) + visual_shape = self.sim.create_visual_shape(self.sim.GEOM_SPHERE, radius=radius, rgba_color=color) + sphere = self.sim.create_body(mass=mass, collision_shape_id=collision_shape, visual_shape_id=visual_shape, + position=position) if mass == 0.0: - self.immovable_objects[sphere] = 'sphere' + self.immovable_bodies[sphere] = 'sphere' else: - self.movable_objects[sphere] = 'sphere' + self.movable_bodies[sphere] = 'sphere' return sphere - def loadVisualBox(self, position, orientation=(0,0,0,1), dimensions=(1.,1.,1.), color=(1,1,1,1)): + def load_visual_box(self, position, orientation=(0, 0, 0, 1), dimensions=(1., 1., 1.), color=None): """ Load a visual box in the world (only available in the simulator). Args: position (float[3]): position of the box in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation of the box using quaternion. If np.quaternion then it - uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w). + orientation (float[4]): orientation of the box using quaternion [x,y,z,w]. dimensions (float[3]): dimensions of the box color (int[4]): color of the box (by default: white and opaque) @@ -1351,23 +1416,18 @@ class World(object): int: unique id of the box in the world """ dimensions = np.array(dimensions) / 2. - visualShape = self.sim.createVisualShape(self.sim.GEOM_BOX, halfExtents=dimensions, rgbaColor = color) - orientation = self.quaternion_converter.convertFrom(orientation) # convert to list - box = self.sim.createMultiBody(baseMass = 0., - baseVisualShapeIndex = visualShape, - basePosition = position, - baseOrientation = orientation) + visual_shape = self.sim.create_visual_shape(self.sim.GEOM_BOX, half_extents=dimensions, rgba_color=color) + box = self.sim.create_body(mass=0., visual_shape_id=visual_shape, position=position, orientation=orientation) self.visual_objects[box] = 'box' return box - def loadBox(self, position, orientation=(0,0,0,1), mass=1., dimensions=(1.,1.,1.), color=(1,1,1,1)): + def load_box(self, position, orientation=(0, 0, 0, 1), mass=1., dimensions=(1., 1., 1.), color=None): """ Load a box in the world (only available in the simulator). Args: position (float[3]): position of the box in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation of the box using quaternion. If np.quaternion then it - uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w). + orientation (float[4]): orientation of the box using quaternion [x,y,z,w]. mass (float): mass of the box (in kg). If mass = 0, the box won't move even if there is a collision. dimensions (float[3]): dimensions of the box color (int[4]): color of the box (by default: white and opaque) @@ -1376,28 +1436,25 @@ class World(object): int: unique id of the box in the world """ dimensions = np.array(dimensions) / 2. - collisionShape = self.sim.createCollisionShape(self.sim.GEOM_BOX, halfExtents=dimensions) - visualShape = self.sim.createVisualShape(self.sim.GEOM_BOX, halfExtents=dimensions, rgbaColor=color) - orientation = self.quaternion_converter.convertFrom(orientation) # convert to list - box = self.sim.createMultiBody(baseMass = mass, - baseCollisionShapeIndex = collisionShape, - baseVisualShapeIndex = visualShape, - basePosition = position, - baseOrientation = orientation) + collision_shape = self.sim.create_collision_shape(self.sim.GEOM_BOX, half_extents=dimensions) + visual_shape = self.sim.create_visual_shape(self.sim.GEOM_BOX, half_extents=dimensions, rgba_color=color) + + box = self.sim.create_body(mass=mass, collision_shape_id=collision_shape, visual_shape_id=visual_shape, + position=position, orientation=orientation) + if mass == 0.0: - self.immovable_objects[box] = 'box' + self.immovable_bodies[box] = 'box' else: - self.movable_objects[box] = 'box' + self.movable_bodies[box] = 'box' return box - def loadVisualCylinder(self, position, orientation=(0,0,0,1), radius=0.5, height=1., color=(1,1,1,1)): + def load_visual_cylinder(self, position, orientation=(0, 0, 0, 1), radius=0.5, height=1., color=None): """ Load a visual cylinder in the world (only available in the simulator). Args: position (float[3]): position of the cylinder in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation of the cylinder using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation of the cylinder using quaternion [x,y,z,w]. radius (float): radius of the cylinder (in meters) height (float): height of the cylinder (in meters) color (int[4]): color of the cylinder (by default: white and opaque) @@ -1405,24 +1462,21 @@ class World(object): Returns: int: unique id of the cylinder in the world """ - visualShape = self.sim.createVisualShape(self.sim.GEOM_CYLINDER, radius=radius, length=height, - rgbaColor = color) - orientation = self.quaternion_converter.convertFrom(orientation) # convert to list - cylinder = self.sim.createMultiBody(baseMass = 0., - baseVisualShapeIndex = visualShape, - basePosition = position, - baseOrientation = orientation) + visual_shape = self.sim.create_visual_shape(self.sim.GEOM_CYLINDER, radius=radius, length=height, + rgba_color=color) + + cylinder = self.sim.create_body(mass=0., visual_shape_id=visual_shape, position=position, + orientation=orientation) self.visual_objects[cylinder] = 'cylinder' return cylinder - def loadCylinder(self, position, orientation=(0,0,0,1), mass=1., radius=0.5, height=1., color=(1,1,1,1)): + def load_cylinder(self, position, orientation=(0, 0, 0, 1), mass=1., radius=0.5, height=1., color=None): """ Load a cylinder in the world (only available in the simulator). Args: position (float[3]): position of the cylinder in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation of the cylinder using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation of the cylinder using quaternion [x,y,z,w]. mass (float): mass of the cylinder (in kg). If mass = 0, it won't move even if there is a collision. radius (float): radius of the cylinder (in meters) height (float): height of the cylinder (in meters) @@ -1431,29 +1485,26 @@ class World(object): Returns: int: unique id of the cylinder in the world """ - collisionShape = self.sim.createCollisionShape(self.sim.GEOM_CYLINDER, radius=radius, height=height) - visualShape = self.sim.createVisualShape(self.sim.GEOM_CYLINDER, radius=radius, length=height, - rgbaColor = color) - orientation = self.quaternion_converter.convertFrom(orientation) # convert to list - cylinder = self.sim.createMultiBody(baseMass = mass, - baseCollisionShapeIndex = collisionShape, - baseVisualShapeIndex = visualShape, - basePosition = position, - baseOrientation = orientation) + collision_shape = self.sim.create_collision_shape(self.sim.GEOM_CYLINDER, radius=radius, height=height) + visual_shape = self.sim.create_visual_shape(self.sim.GEOM_CYLINDER, radius=radius, length=height, + rgba_color=color) + + cylinder = self.sim.create_body(mass=mass, collision_shape_id=collision_shape, visual_shape_id=visual_shape, + position=position, orientation=orientation) + if mass == 0.0: - self.immovable_objects[cylinder] = 'cylinder' + self.immovable_bodies[cylinder] = 'cylinder' else: - self.movable_objects[cylinder] = 'cylinder' + self.movable_bodies[cylinder] = 'cylinder' return cylinder - def loadVisualCapsule(self, position, orientation=(0,0,0,1), radius=0.5, height=1., color=(1,1,1,1)): + def load_visual_capsule(self, position, orientation=(0, 0, 0, 1), radius=0.5, height=1., color=None): """ Load a visual capsule in the world (only available in the simulator). Args: position (float[3]): position of the capsule in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation of the capsule using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation of the capsule using quaternion [x,y,z,w]. radius (float): radius of the capsule (in meters) height (float): height of the capsule (in meters) color (int[4]): color of the capsule (by default: white and opaque) @@ -1462,24 +1513,22 @@ class World(object): int: unique id of the capsule in the world """ height = height/2. - visualShape = self.sim.createVisualShape(self.sim.GEOM_CAPSULE, radius=radius, length=height, - rgbaColor=color) - orientation = self.quaternion_converter.convertFrom(orientation) # convert to list - capsule = self.sim.createMultiBody(baseMass = 0., - baseVisualShapeIndex = visualShape, - basePosition = position, - baseOrientation = orientation) + visual_shape = self.sim.create_visual_shape(self.sim.GEOM_CAPSULE, radius=radius, length=height, + rgba_color=color) + + capsule = self.sim.create_body(mass=0., visual_shape_id=visual_shape, position=position, + orientation=orientation) + self.visual_objects[capsule] = 'capsule' return capsule - def loadCapsule(self, position, orientation=(0,0,0,1), mass=1., radius=0.5, height=1., color=(1,1,1,1)): + def load_capsule(self, position, orientation=(0, 0, 0, 1), mass=1., radius=0.5, height=1., color=None): """ Load a capsule in the world (only available in the simulator). Args: position (float[3]): position of the capsule in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation of the capsule using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation of the capsule using quaternion [x,y,z,w]. mass (float): mass of the capsule (in kg). If mass = 0, it won't move even if there is a collision. radius (float): radius of the capsule (in meters) height (float): height of the capsule (in meters) @@ -1489,23 +1538,20 @@ class World(object): int: unique id of the capsule in the world """ height = height / 2. - collisionShape = self.sim.createCollisionShape(self.sim.GEOM_CAPSULE, radius=radius, height=height) - visualShape = self.sim.createVisualShape(self.sim.GEOM_CAPSULE, radius=radius, length=height, - rgbaColor = color) - orientation = self.quaternion_converter.convertFrom(orientation) # convert to list - capsule = self.sim.createMultiBody(baseMass = mass, - baseCollisionShapeIndex = collisionShape, - baseVisualShapeIndex = visualShape, - basePosition = position, - baseOrientation = orientation) + collision_shape = self.sim.create_collision_shape(self.sim.GEOM_CAPSULE, radius=radius, height=height) + visual_shape = self.sim.create_visual_shape(self.sim.GEOM_CAPSULE, radius=radius, length=height, + rgba_color=color) + + capsule = self.sim.create_body(mass=mass, collision_shape_id=collision_shape, visual_shape_id=visual_shape, + position=position, orientation=orientation) if mass == 0.0: - self.immovable_objects[capsule] = 'capsule' + self.immovable_bodies[capsule] = 'capsule' else: - self.movable_objects[capsule] = 'capsule' + self.movable_bodies[capsule] = 'capsule' return capsule - def loadVisualMesh(self, filename, position, orientation=(0,0,0,1), scale=(1.,1.,1.), color=(1,1,1,1), - objectType='mesh'): + def load_visual_mesh(self, filename, position, orientation=(0, 0, 0, 1), scale=(1., 1., 1.), color=None, + object_type='mesh'): """ Load a visual mesh in the world (only available in the simulator). @@ -1513,26 +1559,20 @@ class World(object): filename (str): path to file for the mesh. Currently, only Wavefront .obj. It will create convex hulls for each object (marked as 'o') in the .obj file. position (float[3]): position of the mesh in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation of the mesh using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation of the mesh using quaternion [x,y,z,w]. scale (float[3]): scale the mesh in the (x,y,z) directions color (int[4]): color of the mesh (by default: white and opaque) Returns: int: unique id of the mesh in the world """ - visualShape = self.sim.createVisualShape(self.sim.GEOM_MESH, fileName=filename, meshScale=scale, - rgbaColor=color) - orientation = self.quaternion_converter.convertFrom(orientation) # convert to list - mesh = self.sim.createMultiBody(baseMass = 0., - baseVisualShapeIndex = visualShape, - basePosition = position, - baseOrientation = orientation) - self.visual_objects[mesh] = objectType + mesh = self.sim.load_mesh(filename, position, orientation, mass=0., scale=scale, color=color, + with_collision=False) + self.visual_objects[mesh] = object_type return mesh - def loadMesh(self, filename, position, orientation=(0,0,0,1), mass=1., scale=(1.,1.,1.), color=(1,1,1,1), - flags=None, objectType='mesh'): + def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None, + flags=None, object_type='mesh'): """ Load a mesh in the world (only available in the simulator). @@ -1540,8 +1580,7 @@ class World(object): filename (str): path to file for the mesh. Currently, only Wavefront .obj. It will create convex hulls for each object (marked as 'o') in the .obj file. position (float[3]): position of the mesh in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation of the mesh using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation of the mesh using quaternion [x,y,z,w]. mass (float): mass of the mesh (in kg). If mass = 0, it won't move even if there is a collision. scale (float[3]): scale the mesh in the (x,y,z) directions color (int[4]): color of the mesh (by default: white and opaque) @@ -1551,58 +1590,45 @@ class World(object): Returns: int: unique id of the mesh in the world """ - if flags is None: - collisionShape = self.sim.createCollisionShape(self.sim.GEOM_MESH, fileName=filename, meshScale=scale) - else: - collisionShape = self.sim.createCollisionShape(self.sim.GEOM_MESH, fileName=filename, meshScale=scale, - flags=flags) - visualShape = self.sim.createVisualShape(self.sim.GEOM_MESH, fileName=filename, meshScale=scale, - rgbaColor=color) - orientation = self.quaternion_converter.convertFrom(orientation) # convert to list - mesh = self.sim.createMultiBody(baseMass = mass, - baseCollisionShapeIndex=collisionShape, - baseVisualShapeIndex = visualShape, - basePosition = position, - baseOrientation = orientation) + mesh = self.sim.load_mesh(filename, position, orientation, mass, scale, color, with_collision=True, + flags=flags) if mass == 0.0: - self.immovable_objects[mesh] = objectType + self.immovable_bodies[mesh] = object_type else: - self.movable_objects[mesh] = objectType + self.movable_bodies[mesh] = object_type return mesh # The following commented code does not work currently because URDF_GEOM_PLANE is not set in Bullet # Note that a plane can be seen as a thin box. - # def loadVisualPlane(self, position, orientation, normal=(0.,0.,1.), color=(1,1,1,1)): + # def load_visual_plane(self, position, orientation, normal=(0.,0.,1.), color=(1,1,1,1)): # """ # Load a visual plane in the world (only available in the simulator). # # Args: # position (float[3]): position of the plane in the Cartesian world space (in meters) - # orientation (float[4], np.quaternion): orientation of the plane using quaternion. - # If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + # orientation (float[4]): orientation of the plane using quaternion [x,y,z,w]. # normal (float[3]): normal to the plane # color (int[4]): color of the plane (by default: white and opaque) # # Returns: # int: unique id of the plane in the world # """ - # visualShape = self.sim.createVisualShape(self.sim.GEOM_PLANE, planeNormal=normal, rgbaColor=color) - # orientation = self.quaternion_converter.convertFrom(orientation) # convert to list - # plane = self.sim.createMultiBody(baseMass=0., - # baseVisualShapeIndex=visualShape, - # basePosition=position, - # baseOrientation=orientation) + # visual_shape = self.sim.create_visual_shape(self.sim.GEOM_PLANE, planeNormal=normal, rgba_color=color) + # + # plane = self.sim.create_body(mass=0., + # visual_shape_id=visual_shape, + # position=position, + # orientation=orientation) # self.visual_objects[plane] = 'plane' # return plane # - # def loadPlane(self, position, orientation, mass=1., normal=(0.,0.,1.), color=(1,1,1,1)): + # def load_plane(self, position, orientation, mass=1., normal=(0.,0.,1.), color=(1,1,1,1)): # """ # Load a plane in the world (only available in the simulator). # # Args: # position (float[3]): position of the plane in the Cartesian world space (in meters) - # orientation (float[4], np.quaternion): orientation of the plane using quaternion. - # If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + # orientation (float[4]): orientation of the plane using quaternion [x,y,z,w]. # mass (float): mass of the plane (in kg). If mass = 0, it won't move even if there is a collision. # normal (float[3]): normal to the plane # color (int[4]): color of the plane (by default: white and opaque) @@ -1610,45 +1636,44 @@ class World(object): # Returns: # int: unique id of the plane in the world # """ - # collisionShape = self.sim.createCollisionShape(self.sim.GEOM_PLANE, planeNormal=normal) - # visualShape = self.sim.createVisualShape(self.sim.GEOM_PLANE, planeNormal=normal, rgbaColor=color) - # orientation = self.quaternion_converter.convertFrom(orientation) # convert to list - # plane = self.sim.createMultiBody(baseMass=mass, - # baseCollisionShapeIndex=collisionShape, - # baseVisualShapeIndex=visualShape, - # basePosition=position, - # baseOrientation=orientation) + # collision_shape = self.sim.create_collision_shape(self.sim.GEOM_PLANE, planeNormal=normal) + # visual_shape = self.sim.create_visual_shape(self.sim.GEOM_PLANE, planeNormal=normal, rgba_color=color) + # + # plane = self.sim.create_body(mass=mass, + # collision_shape_id=collision_shape, + # visual_shape_id=visual_shape, + # position=position, + # orientation=orientation) # if mass == 0.0: - # self.immovable_objects[plane] = 'plane' + # self.immovable_bodies[plane] = 'plane' # else: - # self.movable_objects[plane] = 'plane' + # self.movable_bodies[plane] = 'plane' # return plane # Temporary because the code above doesn't work - def loadPlane(self, position=(0.,0.,0.), orientation=(0.,0.,0.,1.), scaling=1.): + def load_plane(self, position=(0., 0., 0.), orientation=(0., 0., 0., 1.), scale=1.): """ Load a plane in the world (only available in the simulator) Args: position (float[3]): position of the plane orientation (float[4]): orientation of the plane - scaling (float): scaling of the plane + scale (float): scale factor of the plane Returns: int: unique id of the plane """ - plane = self.sim.loadURDF('plane.urdf', position, orientation, useFixedBase=True, globalScaling=scaling) - self.immovable_objects[plane] = plane + plane = self.sim.load_urdf('plane.urdf', position, orientation, use_fixed_base=True, scale=scale) + self.immovable_bodies[plane] = plane return plane - def loadVisualEllipsoid(self, position, orientation=(0,0,0,1), scale=(1., 1., 1.), color=(1,1,1,1)): + def load_visual_ellipsoid(self, position, orientation=(0, 0, 0, 1), scale=(1., 1., 1.), color=None): """ Load a visual ellipsoid (using a mesh) in the world (only available in the simulator). Args: position (float[3]): position in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation using quaternion [x,y,z,w]. scale (float[3]): scale in the (x,y,z) directions color (int[4]): color (by default: white and opaque) @@ -1656,16 +1681,15 @@ class World(object): int: unique id of the ellipsoid in the world """ filename = os.path.dirname(__file__) + '/meshes/ellipsoid.obj' - return self.loadVisualMesh(filename, position, orientation, scale=scale, color=color) + return self.load_visual_mesh(filename, position, orientation, scale=scale, color=color) - def loadEllipsoid(self, position, orientation=(0,0,0,1), mass=1., scale=(1., 1., 1.), color=(1,1,1,1)): + def load_ellipsoid(self, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None): """ Load a ellipsoid (using a mesh) in the world (only available in the simulator). Args: position (float[3]): position in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation using quaternion [x,y,z,w]. mass (float): mass [kg] scale (float[3]): scale in the (x,y,z) directions color (int[4]): color (by default: white and opaque) @@ -1674,16 +1698,16 @@ class World(object): int: unique id of the ellipsoid in the world """ filename = os.path.dirname(__file__) + '/meshes/ellipsoid.obj' - return self.loadMesh(filename, position, orientation, mass=mass, scale=scale, color=color) + return self.load_mesh(filename, position, orientation, mass=mass, scale=scale, color=color) - def loadVisualRightTriangularPrism(self, position, orientation=(0,0,0,1), scale=(1., 1., 1.), color=(1,1,1,1)): + def load_visual_right_triangular_prism(self, position, orientation=(0, 0, 0, 1), scale=(1., 1., 1.), + color=None): """ Load a visual right triangular prism (using a mesh) in the world (only available in the simulator). Args: position (float[3]): position in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation using quaternion [x,y,z,w]. scale (float[3]): scale in the (x,y,z) directions color (int[4]): color (by default: white and opaque) @@ -1691,16 +1715,16 @@ class World(object): int: unique id of the triangular prism in the world """ filename = os.path.dirname(__file__) + '/meshes/right_triangular_prism.obj' - return self.loadVisualMesh(filename, position, orientation, scale=scale, color=color) + return self.load_visual_mesh(filename, position, orientation, scale=scale, color=color) - def loadRightTriangularPrism(self, position, orientation=(0,0,0,1), mass=1., scale=(1., 1., 1.), color=(1,1,1,1)): + def load_right_triangular_prism(self, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), + color=None): """ Load a right triangular prism (using a mesh) in the world (only available in the simulator). Args: position (float[3]): position in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation using quaternion [x,y,z,w]. mass (float): mass [kg] scale (float[3]): scale in the (x,y,z) directions color (int[4]): color (by default: white and opaque) @@ -1709,16 +1733,15 @@ class World(object): int: unique id of the triangular prism in the world """ filename = os.path.dirname(__file__) + '/meshes/right_triangular_prism.obj' - return self.loadMesh(filename, position, orientation, mass=mass, scale=scale, color=color) + return self.load_mesh(filename, position, orientation, mass=mass, scale=scale, color=color) - def loadVisualCone(self, position, orientation=(0,0,0,1), scale=(1., 1., 1.), color=(1,1,1,1)): + def load_visual_cone(self, position, orientation=(0, 0, 0, 1), scale=(1., 1., 1.), color=None): """ Load a visual cone (using a mesh) in the world (only available in the simulator). Args: position (float[3]): position in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation using quaternion [x,y,z,w]. scale (float[3]): scale in the (x,y,z) directions color (int[4]): color (by default: white and opaque) @@ -1726,16 +1749,16 @@ class World(object): int: unique id of the cone in the world """ filename = os.path.dirname(__file__) + '/meshes/cone.obj' - return self.loadVisualMesh(filename, position, orientation, scale=scale, color=color) + return self.load_visual_mesh(filename, position, orientation, scale=scale, color=color) - def loadCone(self, position, orientation=(0,0,0,1), mass=1., scale=(1.,1.,1.), color=(1,1,1,1)): + def load_cone(self, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None): """ Load a visual cone (using a mesh) in the world (only available in the simulator). Args: position (float[3]): position in the Cartesian world space (in meters) - orientation (float[4], np.quaternion): orientation using quaternion. - If np.quaternion then it uses the convention (w,x,y,z). If float[4], it uses the convention (x,y,z,w) + orientation (float[4]): orientation using quaternion [x,y,z,w]. + mass (float): mass [kg] scale (float[3]): scale in the (x,y,z) directions color (int[4]): color (by default: white and opaque) @@ -1743,24 +1766,24 @@ class World(object): int: unique id of the cone in the world """ filename = os.path.dirname(__file__) + '/meshes/cone.obj' - return self.loadMesh(filename, position, orientation, mass=mass, scale=scale, color=color) + return self.load_mesh(filename, position, orientation, mass=mass, scale=scale, color=color) - def loadVisualArrow(self, position, orientation=(0,0,0,1), scale=(1.,1.,1.), color=(1,1,1,1)): + def load_visual_arrow(self, position, orientation=(0, 0, 0, 1), scale=(1., 1., 1.), color=None): pass - def loadArrow(self, position, orientation=(0,0,0,1), mass=1., scale=(1.,1.,1.), color=(1,1,1,1)): + def load_arrow(self, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None): pass def distribute_objects(self, distributor, objects): pass - def getDynamicsInfo(self, bodyId, linkId=-1): + def get_dynamics_info(self, body_id, link_id=-1): """ Return the dynamics information about objects that are in the world. Args: - bodyId (int): object unique id. - linkId (int): link index (or -1 for the base). + body_id (int): object unique id. + link_id (int): link index (or -1 for the base). Returns: float: mass in kg @@ -1774,39 +1797,50 @@ class World(object): float: -1 if not available, damping of contact constraints float: -1 if not available, stiffness contact constraints """ - info = self.sim.getDynamicsInfo(bodyId, linkId) - local_inertia_diag, local_inertial_pos, local_inertial_orn = info[2:5] - local_inertia_diag = np.array(local_inertia_diag) - local_inertial_pos = np.array(local_inertial_pos) - local_inertial_orn = np.array(local_inertial_orn) - return info[:2] + [local_inertia_diag, local_inertial_pos, local_inertial_orn] + info[5:] + return self.sim.get_dynamics_info(body_id, link_id) def print_dynamics_info(self, body_id, link_id=-1): """ Print the dynamics information related to the given body id and link id. Args: - bodyId (int): object unique id. - linkId (int): link index (or -1 for the base). + body_id (int): object unique id. + link_id (int): link index (or -1 for the base). """ - info = self.sim.getDynamicsInfo(body_id, link_id) + info = self.sim.get_dynamics_info(body_id, link_id) print("Mass: {}".format(info[0])) print("Lateral friction coefficient: {}".format(info[1])) print("Local inertia diagonal: {}".format(info[2])) print("Local inertial position: {}".format(info[3])) print("Local inertial orientation (quat=[x,y,z,w]): {}".format(info[4])) - print("Restitution coefficient (bouncyness): {}".format(info[5])) + print("Restitution coefficient (bounciness): {}".format(info[5])) print("Rolling friction coefficient: {}".format(info[6])) print("Spinning friction coefficient: {}".format(info[7])) print("Contact damping coefficient (-1 if not available): {}".format(info[8])) print("Contact stiffness coefficient (-1 if not available): {}".format(info[9])) - def changeDynamics(self, lateral_friction, spinning_friction, rolling_friction, linear_damping, angular_damping, - contact_stiffness=-1, contact_damping=-1): - self.sim.changeDynamics(bodyUniqueId=self.floor_id, linkIndex=-1, lateralFriction=lateral_friction, - spinningFriction=spinning_friction, rollingFriction=rolling_friction, - linearDamping=linear_damping, angularDamping=angular_damping, - contactStiffness=contact_stiffness, contactDamping=contact_damping) + def change_dynamics(self, lateral_friction=1., spinning_friction=0., rolling_friction=0., restitution=0., + linear_damping=0.04, angular_damping=0.04, contact_stiffness=-1, contact_damping=-1, **kwargs): + """ + Change the world floor dynamics. + + Args: + lateral_friction (float): lateral (linear) contact friction + spinning_friction (float): torsional friction around the contact normal + rolling_friction (float): torsional friction orthogonal to contact normal + restitution (float): bounciness of contact. Keep it a bit less than 1. + linear_damping (float): linear damping of the link. + angular_damping (float): angular damping of the link. + contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping` + contact_damping (float): damping of the contact constraints for this body/link. Used together with + `contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact + section. + """ + self.sim.change_dynamics(body_id=self.floor_id, link_id=-1, lateral_friction=lateral_friction, + spinning_friction=spinning_friction, rolling_friction=rolling_friction, + restitution=restitution, linear_damping=linear_damping, + angular_damping=angular_damping, contact_stiffness=contact_stiffness, + contact_damping=contact_damping) class BasicWorld(World): @@ -1815,18 +1849,19 @@ class BasicWorld(World): It creates a basic world with a floor and set the gravity. """ - def __init__(self, simulator, floor_path=None, gravity=(0., 0., -9.81), scaling=1., lateral_friction=.9, + def __init__(self, simulator, floor_path=None, gravity=(0., 0., -9.81), scaling=1., lateral_friction=1., spinning_friction=0., rolling_friction=0., contact_stiffness=-1, contact_damping=-1): super(BasicWorld, self).__init__(simulator, gravity=gravity) if floor_path is None: - self.loadFloor(scaling=scaling) - self.changeDynamics(lateral_friction=lateral_friction, spinning_friction=spinning_friction, - rolling_friction=rolling_friction, linear_damping=0, angular_damping=0, - contact_stiffness=contact_stiffness, contact_damping=contact_damping) - self.simulator.setDefaultContactERP(0.9) + self.load_floor(scaling=scaling) + self.change_dynamics(lateral_friction=lateral_friction, spinning_friction=spinning_friction, + rolling_friction=rolling_friction, linear_damping=0.04, angular_damping=0.04, + contact_stiffness=contact_stiffness, contact_damping=contact_damping) + # self.simulator.setDefaultContactERP(0.9) + self.simulator.set_physics_properties(erp=0.9) else: - self.loadTerrain(floor_path, replace_floor=True) + self.load_terrain(floor_path, replace_floor=True) self.print_dynamics_info(self.floor_id) @@ -1863,11 +1898,11 @@ if __name__ == '__main__': # create world world = BasicWorld(sim) # world = World(sim) - # world.loadBotLab() + # world.load_bot_lab() - # world.loadSDF('/home/brian/Downloads/cobblestones_origin/model.sdf', scaling=1) + # world.load_sdf('/home/brian/Downloads/cobblestones_origin/model.sdf', scaling=1) - # world.loadMesh('/home/brian/Downloads/cobblestones_origin/mesh/cobblestones.obj', + # world.load_mesh('/home/brian/Downloads/cobblestones_origin/mesh/cobblestones.obj', # position=[0, 0, 0], # orientation=[.707, 0, 0, .707], # mass=0., @@ -1875,7 +1910,7 @@ if __name__ == '__main__': # # color=[1, 0, 0, 1], # flags=1) - # world.loadMesh('/home/brian/save/code/random-terrain-generator-master/terrain.obj', + # world.load_mesh('/home/brian/save/code/random-terrain-generator-master/terrain.obj', # position=[0, 0, -2], # orientation=[.707, 0, 0, .707], # mass=0., @@ -1883,67 +1918,67 @@ if __name__ == '__main__': # # color=[1, 0, 0, 1], # flags=1) - # world.loadMesh('bedroom.obj', [0, 0, 0], mass=0., color=[0.4, 0.4, 0.4, 1], flags=1) #, scale=(0.01, 0.01, 0.01)) - # world.loadMesh('mtsthelens.obj', [0, 0, -8], mass=0., color=[0.2, 0.5, 0.2, 1], flags=1, scale=(0.01,0.01,0.01)) - # world.loadMesh('meshes/terrain.obj', [0,0,0], mass=0., color=[1,1,1,1], flags=1) - # world.loadMesh('/home/brian/heightmap_old.obj', [0,0,0], mass=0., scale=(0.1,0.1,0.01), color=[1,1,1,1], flags=1) - # world.loadMesh('/home/brian/Downloads/arab_desert/desert.obj', + # world.load_mesh('bedroom.obj', [0, 0, 0], mass=0., color=[0.4, 0.4, 0.4, 1], flags=1) #, scale=(0.01, 0.01, 0.01)) + # world.load_mesh('mtsthelens.obj', [0, 0, -8], mass=0., color=[0.2, 0.5, 0.2, 1], flags=1, scale=(0.01,0.01,0.01)) + # world.load_mesh('meshes/terrain.obj', [0,0,0], mass=0., color=[1,1,1,1], flags=1) + # world.load_mesh('/home/brian/heightmap_old.obj', [0,0,0], mass=0., scale=(0.1,0.1,0.01), color=[1,1,1,1], flags=1) + # world.load_mesh('/home/brian/Downloads/arab_desert/desert.obj', # position=[0, 0, -10.8], orientation=(0.707,0,0,0.707), mass=0., scale=(1, 1, 1), # color=[1, 1, 1, 1], flags=1) - # world.loadMesh('/home/brian/PhD-repos/pyrobolearn/tests/heightmap_test_exp.obj', [0, 0, 0], mass=0., + # world.load_mesh('/home/brian/PhD-repos/pyrobolearn/tests/heightmap_test_exp.obj', [0, 0, 0], mass=0., # scale=(0.1, 0.1, 0.015), # color=[1, 0, 0, 1], # flags=1) - # world.loadRobot('Cogimon', position=[0,0,1.]) + # world.load_robot('Cogimon', position=[0,0,1.]) - # world.loadRobot('coman', useFixedBase=False) - # sphere = world.loadVisualSphere([1.,0,1.], color=(1,0,0,0.5)) - # world.loadVisualBox([-1,0,1], dimensions=[1.,1.,1.], color=[0,0,1,0.5]) - # world.loadCylinder([0, -1, 1], color=[1, 0, 0, 1]) - # world.loadCapsule([0, 1, 1], color=[1, 0, 0, 1]) - # world.loadMesh(filename='duck.obj', [1, 0, 2], [0.707, 0, 0, 0.707], mass=0.1, scale=[0.1,0.1,0.1], + # world.load_robot('coman', use_fixed_base=False) + # sphere = world.load_visual_Sphere([1.,0,1.], color=(1,0,0,0.5)) + # world.load_visual_box([-1,0,1], dimensions=[1.,1.,1.], color=[0,0,1,0.5]) + # world.load_cylinder([0, -1, 1], color=[1, 0, 0, 1]) + # world.load_capsule([0, 1, 1], color=[1, 0, 0, 1]) + # world.load_mesh(filename='duck.obj', [1, 0, 2], [0.707, 0, 0, 0.707], mass=0.1, scale=[0.1,0.1,0.1], # color=[1, 0, 0, 1]) # from utils.orientation import RotX, RotY, RotZ, getQuaternionFromMatrix # R = RotZ(np.deg2rad(90.)) # q = tuple(getQuaternionFromMatrix(R)) - # world.loadEllipsoid([0,0,2], orientation=q, mass=0, scale=[2.,1.,1.], color=(0,0,1,1)) - # world.loadCone([1,1,2]) - world.loadRightTriangularPrism([-1,-1,2]) - # floor = world.loadMesh(filename='box', [1, 0, 2], mass=0, color=(1, 1, 1, 1)) + # world.load_ellipsoid([0,0,2], orientation=q, mass=0, scale=[2.,1.,1.], color=(0,0,1,1)) + # world.load_cone([1,1,2]) + world.load_right_triangular_prism([-1, -1, 2]) + # floor = world.load_mesh(filename='box', [1, 0, 2], mass=0, color=None) - # floor = world.loadFloor() - # print(p.getDynamicsInfo(floor, -1)) - # floor = world.loadMesh([1,0,0], [0,0,0,1], filename='grass.obj', mass=0, color=(1,1,1,1)) - # texture = sim.loadTexture('grass.png') - # sim.changeVisualShape(floor, -1, textureUniqueId=texture) + # floor = world.load_floor() + # print(p.get_dynamics_info(floor, -1)) + # floor = world.load_mesh([1,0,0], [0,0,0,1], filename='grass.obj', mass=0, color=(1,1,1,1)) + # texture = sim.load_texture('grass.png') + # sim.change_visual_shape(floor, -1, texture_id=texture) - # grass = sim.loadURDF('grass.urdf') - # texture = sim.loadTexture('grass.png') - # sim.changeVisualShape(grass, -1, textureUniqueId=texture) + # grass = sim.load_urdf('grass.urdf') + # texture = sim.load_texture('grass.png') + # sim.change_visual_shape(grass, -1, texture_id=texture) - # vs = world.loadVisualSphere([0,0,2], radius=0.1, color=(0,0,1,1)) + # vs = world.load_visual_Sphere([0,0,2], radius=0.1, color=(0,0,1,1)) # path = '/home/brian/bullet3/data/' - # objects = p.loadMJCF(path+"MPL/mpl2.xml") + # objects = p.load_mjcf(path+"MPL/mpl2.xml") - # world.loadPlane([1.,0.,1.], [0,0,0,1], color=(1,0,0,1)) + # world.load_plane([1.,0.,1.], [0,0,0,1], color=(1,0,0,1)) T = 1000 w = 2.*np.pi / T red = True # loop for t in count(): - # p = world.getObjectPosition(sphere) + # p = world.get_object_position(sphere) # p -= 0.001 * np.array([1.,0,0]) p = np.array([np.cos(w*t), np.sin(w*t), 1.]) - # world.moveObject(sphere, p) + # world.move_object(sphere, p) # if t % T == 0: # if red: - # world.changeObjectColor(sphere, (1,0,0,0.5)) + # world.change_object_color(sphere, (1,0,0,0.5)) # else: - # world.changeObjectColor(sphere, (0,0,1,0.5)) + # world.change_object_color(sphere, (0,0,1,0.5)) # red = not red - # world.moveObject(sphere) + # world.move_object(sphere) world.step(sleep_dt=1./240)