mirror of
https://github.com/wassname/pyrobolearn.git
synced 2026-09-09 11:31:38 +08:00
update robots + add shadow hand + units converter
This commit is contained in:
Brian Delhaisse
@@ -7,6 +7,8 @@ from itertools import count
|
||||
from pyrobolearn.simulators import BulletSim
|
||||
from pyrobolearn.worlds import BasicWorld
|
||||
from pyrobolearn.robots import Quadcopter
|
||||
from pyrobolearn.utils.units import rpm_to_rad_per_second
|
||||
|
||||
|
||||
# Create simulator
|
||||
sim = BulletSim()
|
||||
@@ -20,9 +22,9 @@ robot = Quadcopter(sim)
|
||||
# print information about the robot
|
||||
robot.print_info()
|
||||
|
||||
rpm = robot.getStationaryRPM()
|
||||
rpm = robot.get_stationary_rpm()
|
||||
print("Stationary RPM: {}".format(rpm))
|
||||
v = robot.rpmToRadPerSecond(rpm+20)
|
||||
v = rpm_to_rad_per_second(rpm+20)
|
||||
v = [v, -v, v, -v]
|
||||
|
||||
# run simulation
|
||||
|
||||
@@ -2,6 +2,7 @@
|
||||
"""Load the RRBot robotic platform.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from itertools import count
|
||||
from pyrobolearn.simulators import BulletSim
|
||||
from pyrobolearn.worlds import BasicWorld
|
||||
@@ -93,7 +94,7 @@ raw_input('press enter')
|
||||
print(robot.get_link_names())
|
||||
force = np.array([1., 0., 0.])
|
||||
pos = np.array([0., 0., 0.])
|
||||
sim.apply_external_force(robot.id, 1, force, pos, flags=p.LINK_FRAME) # link_frame = 1
|
||||
sim.apply_external_force(robot.id, 1, force, pos, frame=1) # link_frame = 1, world_frame = 2
|
||||
|
||||
slider = sim.add_user_debug_parameter('force', -1000., 1000., 0)
|
||||
|
||||
@@ -102,7 +103,7 @@ J = sim.calculate_jacobian(robot.id, 1, [0., 0., 0.])
|
||||
print(np.array(J[0]))
|
||||
|
||||
a = robot.get_joint_positions()
|
||||
# print(robot.getJacobianMatrix(1, np.array([0.,0.]))) # TODO: need to convert numpy array to list
|
||||
# print(robot.get_jacobian(1, np.array([0.,0.]))) # TODO: need to convert numpy array to list
|
||||
|
||||
linkId = 2
|
||||
com_frame = robot.get_link_states(linkId)[2]
|
||||
@@ -144,7 +145,7 @@ for i in range(10000):
|
||||
force = np.array([0., 0., 1.])
|
||||
else:
|
||||
force = np.array([0., 0., 0.])
|
||||
sim.apply_external_force(robot.id, linkId, force, pos, flags=p.LINK_FRAME) # p.LINK_FRAME = 1
|
||||
sim.apply_external_force(robot.id, linkId, force, pos, frame=1) # p.LINK_FRAME = 1, p.WORLD_FRAME = 2
|
||||
|
||||
# robot.update_joint_slider()
|
||||
world.step(sleep_dt=1./240)
|
||||
|
||||
@@ -83,7 +83,6 @@ class ELLA(object):
|
||||
it has been observed that the choice of only updating the current :math:`s^{(t)}` does not affect the quality of
|
||||
the model as the number of tasks grows large.
|
||||
|
||||
|
||||
...
|
||||
|
||||
Warnings:
|
||||
@@ -91,7 +90,11 @@ class ELLA(object):
|
||||
the Hessian matrix of the loss function with respect to the parameters evaluated at the best found above
|
||||
parameters. Depending on the number of parameters that matrix can be big and expensive to compute.
|
||||
- The ELLA assumes that the input and output dimension of the model as well as its number of parameters is
|
||||
constant between the various tasks.
|
||||
constant between the various tasks. For different state and action spaces between the tasks, see inter-task
|
||||
mappings [6], or ELLA using task groups where tasks in the same group share a common state and action space,
|
||||
such that :math:`\theta^{(t)} = B^{(g)}s^{(t)}` with :math:`B^{(g)} = \Phi^{(g)} L` where :math:`g` denotes
|
||||
the task group, :math:`B^{(g)}` is the latent model components shared withing :math:`g`, :math:`L` is the
|
||||
global latent model components, and :math:`\Phi^{(g)}` is the mapping from :math:`L` to :math:`B^{(g)}` [7].
|
||||
- The complexity of each ELLA update is :math:`O(k^2 d^3, \xi(d, n_t))` where :math:`k` is the number of latent
|
||||
components, :math:`d` is the dimensionality of the parameters, :math:`n_t` is the number of data instances (in
|
||||
SL) or trajectories (in RL), and :math:`\xi` is the function that computes the complexity to compute the
|
||||
@@ -115,6 +118,9 @@ class ELLA(object):
|
||||
[3] "Online Multi-Task Learning for Policy Gradient Methods", Ammar et al., 2014
|
||||
[4] Implementation of ELLA on Github (by Paul Ruvolo): https://github.com/paulruvolo/ELLA
|
||||
[5] Implementation of PG-ELLA on Github (by ): https://github.com/cdcsai/Online_Multi_Task_Learning
|
||||
[6] "Transfer learning via inter-task mappings for temporal difference learning", Taylor et al., 2007
|
||||
[7] "Autonomous cross-domain knowledge transfer in lifelong policy gradient reinforcement learning", Ammar et
|
||||
al., 2015
|
||||
"""
|
||||
|
||||
def __init__(self, num_parameters, num_latent_component, l1_sparsity_coefficient=1., l2_library_coefficient=1.):
|
||||
|
||||
@@ -37,7 +37,7 @@ The folder contains different kind of robots including manipulators, legged robo
|
||||
- [Cubli](https://github.com/xinsongyan/cubli)
|
||||
- [Darwin](https://github.com/HumaRobotics/darwin_description)
|
||||
- [e.Do](https://github.com/Comau/eDO_description)
|
||||
- [epuck](https://github.com/gctronic/epuck_driver_cpp)
|
||||
- [E-puck](https://github.com/gctronic/epuck_driver_cpp)
|
||||
- [F10 racecar](https://github.com/erwincoumans/pybullet_robots/tree/master/data/f10_racecar)
|
||||
- [Fetch](https://github.com/fetchrobotics/fetch_ros)
|
||||
- [Franka Emika](https://github.com/frankaemika/franka_ros)
|
||||
@@ -71,6 +71,7 @@ The folder contains different kind of robots including manipulators, legged robo
|
||||
- Sawyer: [1](https://github.com/RethinkRobotics/sawyer_robot), [2](https://github.com/erwincoumans/pybullet_robots)
|
||||
- [SEA hexapod](https://github.com/alexansari101/snake_ws)
|
||||
- [SEA snake]( https://github.com/alexansari101/snake_ws)
|
||||
- [Shadow hand](https://github.com/shadow-robot/sr_common)
|
||||
- [Soft hand](https://github.com/CentroEPiaggio/pisa-iit-soft-hand)
|
||||
- [Swimmer](https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf)
|
||||
- [Valkyrie](https://github.com/openhumanoids/val_description)
|
||||
@@ -83,6 +84,7 @@ The folder contains different kind of robots including manipulators, legged robo
|
||||
Here is a list of robots that I plan to add at one point (some of them require to simulate some fluid dynamics, as done in the `quadcopter` class) but can interest already some people:
|
||||
- [ ] [ANYmal](https://www.anymal-research.org/): I am currently not sure if I can release the URDF of this robot, and thus I will not do it until it is officially released by ETH.
|
||||
- [ ] [ROS robots](https://robots.ros.org/): I plan to provide soon the robots listed on this website (I am currently cleaning the URDFs of some of them)
|
||||
- [ ] [Universal robots](https://github.com/ros-industrial/universal_robot)
|
||||
- [ ] [rotors-simulator](https://github.com/ethz-asl/rotors_simulator)
|
||||
- [ ] [uuv-simulator](https://github.com/uuvsimulator/uuv_simulator)
|
||||
- [ ] [usv-simulator](https://github.com/OUXT-Polaris/ros_ship_packages)
|
||||
|
||||
@@ -76,6 +76,7 @@ from .baxter import Baxter
|
||||
# Hands
|
||||
from .allegrohand import AllegroHand
|
||||
from .softhand import SoftHand
|
||||
from .shadowhand import ShadowHand
|
||||
|
||||
# Wheeled
|
||||
from .epuck import Epuck
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.legged_robot import QuadrupedRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Aibo(QuadrupedRobot):
|
||||
r"""Aibo
|
||||
@@ -13,6 +21,9 @@ class Aibo(QuadrupedRobot):
|
||||
This is the Aibo quadruped dog robot developed by Sony.
|
||||
|
||||
WARNINGS: THE INERTIA MATRICES AND THE POSITION OF COLLISIONS MESHES IN THE URDF NEED TO BE CORRECTED!!
|
||||
|
||||
References:
|
||||
[1] https://github.com/dkotfis/aibo_ros
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
@@ -58,13 +69,13 @@ if __name__ == "__main__":
|
||||
world = BasicWorld(sim)
|
||||
|
||||
# create robot
|
||||
robot = Aibo(sim) # , useFixedBase=True)
|
||||
robot = Aibo(sim) # , fixed_base=True)
|
||||
|
||||
# print information about the robot
|
||||
robot.print_info()
|
||||
|
||||
# # Position control using sliders
|
||||
# robot.add_joint_slider(robot.getLeftFrontLegIds() + robot.getRightFrontLegIds())
|
||||
robot.add_joint_slider(robot.left_front_leg + robot.right_front_leg)
|
||||
|
||||
# run simulator
|
||||
for _ in count():
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.hand import Hand
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
# TODO: correct the inertia matrices and masses; they are too big!
|
||||
class AllegroHand(Hand):
|
||||
@@ -55,7 +63,7 @@ if __name__ == "__main__":
|
||||
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.print_info()
|
||||
@@ -67,7 +75,7 @@ if __name__ == "__main__":
|
||||
|
||||
for i in count():
|
||||
right_hand.update_joint_slider()
|
||||
# right_hand.set_joint_positions([0.] * right_hand.getNumberOfDoFs())
|
||||
# right_hand.set_joint_positions([0.] * right_hand.num_dofs)
|
||||
|
||||
# step in simulation
|
||||
world.step(sleep_dt=1./240)
|
||||
|
||||
@@ -6,9 +6,20 @@ import os
|
||||
|
||||
from pyrobolearn.robots.legged_robot import QuadrupedRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Ant(QuadrupedRobot):
|
||||
r"""Ant Mujoco Model
|
||||
|
||||
References:
|
||||
[1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
@@ -38,7 +49,7 @@ class Ant(QuadrupedRobot):
|
||||
['link0_8', 'link0_10']]]
|
||||
|
||||
self.feet = [self.get_link_ids(link) for link in ['front_left_foot', 'front_right_foot',
|
||||
'left_back_foot', 'right_back_foot']
|
||||
'left_back_foot', 'right_back_foot']
|
||||
if link in self.link_names]
|
||||
|
||||
|
||||
@@ -55,13 +66,13 @@ if __name__ == "__main__":
|
||||
world = BasicWorld(sim)
|
||||
|
||||
# create robot
|
||||
robot = Ant(sim) # , useFixedBase=True)
|
||||
robot = Ant(sim) # , fixed_base=True)
|
||||
|
||||
# print information about the robot
|
||||
robot.print_info()
|
||||
|
||||
# Position control using sliders
|
||||
# robot.add_joint_slider(robot.getLeftFrontLegIds() + robot.getRightFrontLegIds())
|
||||
# robot.add_joint_slider(robot.left_front_leg + robot.right_front_leg)
|
||||
|
||||
# run simulator
|
||||
for _ in count():
|
||||
|
||||
@@ -0,0 +1,91 @@
|
||||
#!/usr/bin/env python
|
||||
"""Provide the ANYmal robotic platform.
|
||||
"""
|
||||
|
||||
import os
|
||||
import numpy as np
|
||||
|
||||
from legged_robot import QuadrupedRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class ANYmal(QuadrupedRobot):
|
||||
r"""ANYmal robot
|
||||
|
||||
'ANYmal is a dog-sized quadrupedal robot weighing about 32 kg. Each leg is about 55 cm long and has three actuated
|
||||
degrees of freedom, namely, hip abduction/adduction, hip flexion/extension, and knee flexion/extension.
|
||||
ANYmal is equipped with 12 SEAs'
|
||||
|
||||
References:
|
||||
[1] "ANYmal - a Highly Mobile and Dynamic Quadrupedal Robot", Hutter et al., 2016
|
||||
[2] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019
|
||||
[3] www.rsl.ethz.ch/robots-media/anymal.html
|
||||
[4] www.anybotics.com/anymal
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
simulator,
|
||||
position=(0, 0, .6),
|
||||
orientation=(0, 0, 0, 1),
|
||||
fixed_base=False,
|
||||
scaling=1.,
|
||||
urdf=os.path.dirname(__file__) + '/urdfs/anymal/anymal.urdf'):
|
||||
# check parameters
|
||||
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(ANYmal, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling)
|
||||
self.name = 'anymal'
|
||||
|
||||
self.legs = [[self.get_link_ids(link) for link in links if link in self.link_names]
|
||||
for links in [['LF_HIP', 'LF_THIGH', 'LF_SHANK'],
|
||||
['RF_HIP', 'RF_THIGH', 'RF_SHANK'],
|
||||
['LH_HIP', 'LH_THIGH', 'LH_SHANK'],
|
||||
['RH_HIP', 'RH_THIGH', 'RH_SHANK']]]
|
||||
|
||||
self.feet = [self.get_link_ids(link) for link in ['LF_FOOT_MOUNT', 'RF_FOOT_MOUNT', 'LH_FOOT_MOUNT',
|
||||
'RH_FOOT_MOUNT'] if link in self.link_names]
|
||||
|
||||
# taken from "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019
|
||||
self.kp = 50. * np.ones(12)
|
||||
self.kd = 0.1 * np.ones(12)
|
||||
|
||||
|
||||
# Test
|
||||
if __name__ == "__main__":
|
||||
from itertools import count
|
||||
from pyrobolearn.simulators import BulletSim
|
||||
from pyrobolearn.worlds import BasicWorld
|
||||
|
||||
# Create simulator
|
||||
sim = BulletSim()
|
||||
|
||||
# create world
|
||||
world = BasicWorld(sim)
|
||||
|
||||
# create robot
|
||||
robot = ANYmal(sim)
|
||||
|
||||
# print information about the robot
|
||||
robot.print_info()
|
||||
|
||||
# Position control using sliders
|
||||
# robot.add_joint_slider(robot.left_front_leg)
|
||||
|
||||
# run simulator
|
||||
for _ in count():
|
||||
# robot.update_joint_slider()
|
||||
world.step(sleep_dt=1./240)
|
||||
@@ -7,6 +7,14 @@ import os
|
||||
from pyrobolearn.robots.legged_robot import BipedRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Atlas(BipedRobot, BiManipulatorRobot):
|
||||
r"""Atlas robot
|
||||
@@ -74,7 +82,7 @@ if __name__ == "__main__":
|
||||
robot.print_info()
|
||||
|
||||
# position control using sliders
|
||||
robot.add_joint_slider(robot.getLeftLegIds())
|
||||
robot.add_joint_slider(robot.left_leg)
|
||||
|
||||
# run simulator
|
||||
for _ in count():
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
# TODO inertia are not corrects
|
||||
class Ballbot(Robot):
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Baxter(BiManipulatorRobot):
|
||||
r"""Baxter robot
|
||||
@@ -13,7 +21,8 @@ class Baxter(BiManipulatorRobot):
|
||||
Baxter robot built by Rethink Robotics.
|
||||
|
||||
References:
|
||||
[1] Rethink Robotics
|
||||
[1] Rethink Robotics: https://www.rethinkrobotics.com/
|
||||
[2] https://github.com/RethinkRobotics/baxter_common
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class BB8(Robot):
|
||||
r"""BB8 robot
|
||||
|
||||
@@ -7,6 +7,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.legged_robot import BipedRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Blackbird(BipedRobot):
|
||||
r"""Blackbird biped robot
|
||||
|
||||
@@ -11,6 +11,14 @@ import sympy.physics.mechanics as mechanics
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
from pyrobolearn.utils.orientation import get_symbolic_matrix_from_axis_angle, get_matrix_from_quaternion
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class CartPole(Robot):
|
||||
r"""CartPole robot
|
||||
@@ -392,7 +400,7 @@ class CartPole(Robot):
|
||||
return M, f
|
||||
|
||||
def linearize_equations_of_motion(self, point=None, verbose=False):
|
||||
"""
|
||||
r"""
|
||||
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
|
||||
a certain point.
|
||||
@@ -473,30 +481,32 @@ class LQR(object):
|
||||
"""
|
||||
|
||||
def __init__(self, A, B, Q=None, R=None, N=None):
|
||||
if not self.isControllable(A, B):
|
||||
if not self.is_controllable(A, B):
|
||||
raise ValueError("The system is not controllable")
|
||||
self.A = A
|
||||
self.B = B
|
||||
if Q is None: Q = np.identity(A.shape[1])
|
||||
if Q is None:
|
||||
Q = np.identity(A.shape[1])
|
||||
self.Q = Q
|
||||
if R is None: R = np.identity(B.shape[1])
|
||||
if R is None:
|
||||
R = np.identity(B.shape[1])
|
||||
self.R = R
|
||||
self.N = N
|
||||
self.K = None
|
||||
|
||||
@staticmethod
|
||||
def isControllable(A, B):
|
||||
def is_controllable(A, B):
|
||||
return np.linalg.matrix_rank(control.ctrb(A, B)) == A.shape[0]
|
||||
|
||||
def getRiccatiSolution(self):
|
||||
def get_riccati_solution(self):
|
||||
S = solve_continuous_are(self.A, self.B, self.Q, self.R, s=self.N)
|
||||
return S
|
||||
|
||||
def getGainK(self):
|
||||
#S = self.getRiccatiSolution()
|
||||
#S1 = self.B.T.dot(S)
|
||||
#if self.N is not None: S1 += self.N.T
|
||||
#K = np.linalg.inv(self.R).dot(S1)
|
||||
def get_gain_k(self):
|
||||
# S = self.get_riccati_solution()
|
||||
# S1 = self.B.T.dot(S)
|
||||
# if self.N is not None: S1 += self.N.T
|
||||
# K = np.linalg.inv(self.R).dot(S1)
|
||||
|
||||
if self.N is None:
|
||||
K, S, E = control.lqr(self.A, self.B, self.Q, self.R)
|
||||
@@ -508,7 +518,7 @@ class LQR(object):
|
||||
"""Return the u."""
|
||||
|
||||
if self.K is None:
|
||||
self.K = self.getGainK()
|
||||
self.K = self.get_gain_k()
|
||||
|
||||
if xd is None:
|
||||
return self.K.dot(x)
|
||||
@@ -543,7 +553,7 @@ if __name__ == "__main__":
|
||||
|
||||
# LQR controller
|
||||
lqr = LQR(A, B)
|
||||
K = lqr.getGainK()
|
||||
K = lqr.get_gain_k()
|
||||
|
||||
for i in count():
|
||||
# control
|
||||
|
||||
@@ -7,6 +7,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.legged_robot import BipedRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Cassie(BipedRobot):
|
||||
r"""Cassie robot
|
||||
|
||||
@@ -9,8 +9,21 @@ from pyrobolearn.robots.legged_robot import QuadrupedRobot
|
||||
from pyrobolearn.robots.wheeled_robot import WheeledRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Centauro(WheeledRobot, QuadrupedRobot, BiManipulatorRobot):
|
||||
r"""Centauro robot
|
||||
|
||||
References:
|
||||
[1] https://github.com/ADVRHumanoids/centauro-simulator
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
simulator,
|
||||
@@ -32,7 +45,7 @@ class Centauro(WheeledRobot, QuadrupedRobot, BiManipulatorRobot):
|
||||
if fixed_base is None:
|
||||
fixed_base = False
|
||||
|
||||
super(Centauro, self).__init__(simulator, urdf, position, orientation, fixed_base)
|
||||
super(Centauro, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling=scaling)
|
||||
self.name = 'centauro'
|
||||
|
||||
self.necks = [self.get_link_ids(link) for link in ['neck_' + str(i) for i in range(1, 4)]]
|
||||
@@ -75,10 +88,10 @@ if __name__ == "__main__":
|
||||
|
||||
# print information about the robot
|
||||
robot.print_info()
|
||||
print("Number of Legs: {}".format(robot.getNumberOfLegs()))
|
||||
print("Number of Arms: {}".format(robot.getNumberOfArms()))
|
||||
print("Number of Legs: {}".format(robot.num_legs))
|
||||
print("Number of Arms: {}".format(robot.num_arms))
|
||||
|
||||
# robot.add_joint_slider(robot.getRightFrontLegIds())
|
||||
robot.add_joint_slider(robot.right_front_leg)
|
||||
robot.drive(speed=3)
|
||||
|
||||
# run simulator
|
||||
|
||||
@@ -7,10 +7,20 @@ import os
|
||||
from pyrobolearn.robots.legged_robot import BipedRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Cogimon(BipedRobot, BiManipulatorRobot):
|
||||
r"""Cogimon humanoid robot.
|
||||
|
||||
References:
|
||||
[1] https://github.com/ADVRHumanoids/iit-cogimon-ros-pkg
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
@@ -53,13 +63,6 @@ class Cogimon(BipedRobot, BiManipulatorRobot):
|
||||
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, position=init_pos, orientation=init_orient, fixed_base=useFixedBase,
|
||||
scaling=scaling, urdf=urdf_path)
|
||||
|
||||
|
||||
# Test
|
||||
if __name__ == "__main__":
|
||||
from itertools import count
|
||||
@@ -73,7 +76,7 @@ if __name__ == "__main__":
|
||||
world = BasicWorld(sim)
|
||||
|
||||
# create robot
|
||||
robot = Cogimon(sim, lower_body=False)
|
||||
robot = Cogimon(sim, lower_body=True)
|
||||
|
||||
# print information about the robot
|
||||
robot.print_info()
|
||||
|
||||
@@ -8,10 +8,20 @@ import numpy as np
|
||||
from pyrobolearn.robots.legged_robot import BipedRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Coman(BipedRobot, BiManipulatorRobot):
|
||||
r"""Coman robot
|
||||
|
||||
References:
|
||||
[1] https://github.com/ADVRHumanoids/iit-coman-ros-pkg
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.legged_robot import HexapodRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Crab(HexapodRobot):
|
||||
r"""Crab Hexapod robot
|
||||
|
||||
@@ -5,6 +5,15 @@
|
||||
import os
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
from pyrobolearn.utils.orientation import get_rpy_from_quaternion
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Cubli(Robot):
|
||||
@@ -50,7 +59,7 @@ class Cubli(Robot):
|
||||
if __name__ == "__main__":
|
||||
import numpy as np
|
||||
from itertools import count
|
||||
from pyrobolearn.simulators import BulletSim, pybullet
|
||||
from pyrobolearn.simulators import BulletSim
|
||||
from pyrobolearn.worlds import BasicWorld
|
||||
|
||||
# Create simulator
|
||||
@@ -77,9 +86,9 @@ if __name__ == "__main__":
|
||||
|
||||
for i in count():
|
||||
# get state
|
||||
quaternion = robot.get_base_orientation(False)
|
||||
quaternion = robot.get_base_orientation()
|
||||
w = robot.get_base_angular_velocity()
|
||||
euler = pybullet.getEulerFromQuaternion(quaternion.tolist())
|
||||
euler = get_rpy_from_quaternion(quaternion)
|
||||
|
||||
# PD control
|
||||
torques = [-Kp * (desired_roll - euler[0]) + Kd * w[0], 0., 0.]
|
||||
|
||||
@@ -8,6 +8,14 @@ import numpy as np
|
||||
from pyrobolearn.robots.legged_robot import BipedRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Darwin(BipedRobot, BiManipulatorRobot):
|
||||
r"""Darwin robot
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Edo(ManipulatorRobot):
|
||||
r"""Edo robot
|
||||
|
||||
@@ -7,6 +7,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.wheeled_robot import DifferentialWheeledRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Epuck(DifferentialWheeledRobot):
|
||||
r"""Epuck robot
|
||||
|
||||
@@ -7,6 +7,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.wheeled_robot import AckermannWheeledRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class F10Racecar(AckermannWheeledRobot):
|
||||
r"""F10Racecar robot
|
||||
@@ -36,7 +44,7 @@ class F10Racecar(AckermannWheeledRobot):
|
||||
self.name = 'racecar'
|
||||
|
||||
self.wheels = [self.get_link_ids(link) for link in ['left_front_wheel', 'right_front_wheel',
|
||||
'left_rear_wheel', 'right_rear_wheel']
|
||||
'left_rear_wheel', 'right_rear_wheel']
|
||||
if link in self.link_names]
|
||||
self.wheel_directions = np.ones(len(self.wheels))
|
||||
|
||||
|
||||
@@ -7,10 +7,21 @@ import os
|
||||
from pyrobolearn.robots.wheeled_robot import WheeledRobot
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Fetch(WheeledRobot, ManipulatorRobot):
|
||||
r"""Fetch robot
|
||||
|
||||
References:
|
||||
[1] Fetch robotics: https://fetchrobotics.com/
|
||||
[2] Fetch description: https://github.com/fetchrobotics/fetch_ros
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Franka(ManipulatorRobot):
|
||||
r"""Franka Emika robot
|
||||
|
||||
@@ -6,9 +6,20 @@ import os
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class HalfCheetah(Robot):
|
||||
r"""Half Cheetah Mujoco Model
|
||||
|
||||
References:
|
||||
[1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
+25
-15
@@ -4,6 +4,15 @@
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__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 Hand(Robot):
|
||||
r"""Hand end-effector
|
||||
@@ -26,10 +35,10 @@ class Hand(Robot):
|
||||
"""Return the number of fingers on the hand"""
|
||||
return len(self.fingers)
|
||||
|
||||
def getFinger(self, fingerId=None):
|
||||
def get_finger(self, finger_id=None):
|
||||
"""Return the list of joint/link ids for the specified finger"""
|
||||
if fingerId:
|
||||
return self.fingers[fingerId]
|
||||
if finger_id:
|
||||
return self.fingers[finger_id]
|
||||
return self.fingers
|
||||
|
||||
|
||||
@@ -37,6 +46,7 @@ class TwoHand(Hand):
|
||||
r"""Two hand end-effectors
|
||||
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
simulator,
|
||||
urdf,
|
||||
@@ -59,20 +69,20 @@ class TwoHand(Hand):
|
||||
"""Return the number of fingers on the right hand"""
|
||||
return len(self.right_fingers)
|
||||
|
||||
def getLeftFingers(self, fingerId=None):
|
||||
def get_left_fingers(self, finger_id=None):
|
||||
"""Return the specified left fingers"""
|
||||
if fingerId:
|
||||
if isinstance(fingerId, int):
|
||||
return self.fingers[self.left_fingers[fingerId]]
|
||||
elif isinstance(fingerId, (tuple, list)):
|
||||
return [self.fingers[self.left_fingers[finger]] for finger in fingerId]
|
||||
if finger_id:
|
||||
if isinstance(finger_id, int):
|
||||
return self.fingers[self.left_fingers[finger_id]]
|
||||
elif isinstance(finger_id, (tuple, list)):
|
||||
return [self.fingers[self.left_fingers[finger]] for finger in finger_id]
|
||||
return [self.fingers[finger] for finger in self.left_fingers]
|
||||
|
||||
def getRightFingers(self, fingerId=None):
|
||||
def get_right_fingers(self, finger_id=None):
|
||||
"""Return the specified right fingers"""
|
||||
if fingerId:
|
||||
if isinstance(fingerId, int):
|
||||
return self.fingers[self.right_fingers[fingerId]]
|
||||
elif isinstance(fingerId, (tuple, list)):
|
||||
return [self.fingers[self.right_fingers[finger]] for finger in fingerId]
|
||||
if finger_id:
|
||||
if isinstance(finger_id, int):
|
||||
return self.fingers[self.right_fingers[finger_id]]
|
||||
elif isinstance(finger_id, (tuple, list)):
|
||||
return [self.fingers[self.right_fingers[finger]] for finger in finger_id]
|
||||
return [self.fingers[finger] for finger in self.right_fingers]
|
||||
|
||||
@@ -6,9 +6,20 @@ import os
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Hopper(Robot):
|
||||
r"""Hopper Mujoco Model
|
||||
|
||||
References:
|
||||
[1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
@@ -8,6 +8,14 @@ from pyrobolearn.robots.legged_robot import BipedRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
from pyrobolearn.robots.hand import TwoHand
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Hubo(BipedRobot, BiManipulatorRobot, TwoHand):
|
||||
r"""Hubo robot
|
||||
|
||||
@@ -7,9 +7,20 @@ import os
|
||||
from pyrobolearn.robots.legged_robot import BipedRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Humanoid(BipedRobot, BiManipulatorRobot):
|
||||
r"""Humanoid Mujoco Model
|
||||
|
||||
References:
|
||||
[1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
@@ -7,6 +7,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.wheeled_robot import DifferentialWheeledRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Husky(DifferentialWheeledRobot):
|
||||
r"""Husky robot
|
||||
@@ -40,7 +48,7 @@ class Husky(DifferentialWheeledRobot):
|
||||
self.name = 'husky'
|
||||
|
||||
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']
|
||||
'rear_left_wheel_link', 'rear_right_wheel_link']
|
||||
if link in self.link_names]
|
||||
self.wheel_directions = np.ones(len(self.wheels))
|
||||
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.legged_robot import QuadrupedRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class HyQ(QuadrupedRobot):
|
||||
r"""HyQ robot
|
||||
@@ -13,7 +21,8 @@ class HyQ(QuadrupedRobot):
|
||||
HyQ robot created by IIT.
|
||||
|
||||
References:
|
||||
[1]
|
||||
[1] https://dls.iit.it/robots/hyq-robot
|
||||
[2] https://github.com/iit-DLSLab/hyq-description
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
@@ -65,7 +74,7 @@ if __name__ == "__main__":
|
||||
robot.print_info()
|
||||
|
||||
# # Position control using sliders
|
||||
robot.add_joint_slider(robot.getLeftFrontLegIds())
|
||||
robot.add_joint_slider(robot.left_front_leg)
|
||||
|
||||
# run simulator
|
||||
for _ in count():
|
||||
|
||||
@@ -7,6 +7,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.legged_robot import QuadrupedRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class HyQ2Max(QuadrupedRobot):
|
||||
r"""HyQ2Max
|
||||
@@ -15,6 +23,8 @@ class HyQ2Max(QuadrupedRobot):
|
||||
|
||||
References:
|
||||
[1] "Design of the Hydraulically-Actuated,Torque-Controlled Quadruped Robot HyQ2Max", Semini et al., 2016
|
||||
[2] https://dls.iit.it/robots/hyq2max
|
||||
[3] https://github.com/iit-DLSLab/hyq2max-description
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
@@ -7,11 +7,22 @@ import os
|
||||
from pyrobolearn.robots.legged_robot import BipedRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class ICub(BipedRobot, BiManipulatorRobot):
|
||||
r"""ICub robot
|
||||
|
||||
References:
|
||||
[1] http://www.icub.org/
|
||||
[2] https://github.com/robotology-playground/icub-models
|
||||
[3] https://github.com/robotology-playground/icub-model-generator
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
@@ -31,7 +42,7 @@ class ICub(BipedRobot, BiManipulatorRobot):
|
||||
if fixed_base is None:
|
||||
fixed_base = False
|
||||
|
||||
super(ICub, self).__init__(simulator, urdf, position, orientation, fixed_base)
|
||||
super(ICub, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling=scaling)
|
||||
self.name = 'icub'
|
||||
|
||||
self.head = self.get_link_ids('head') if 'head' in self.link_names else None
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Jaco(ManipulatorRobot):
|
||||
r"""Jaco (manipulator) robot
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class KR5(ManipulatorRobot):
|
||||
r"""Kuka KR5 sixx R650 robot
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class KukaIIWA(ManipulatorRobot):
|
||||
r"""Kuka IIWA robot
|
||||
@@ -83,7 +91,7 @@ if __name__ == "__main__":
|
||||
dx = robot.get_link_world_linear_velocities(linkId)
|
||||
|
||||
# get (linear) jacobian
|
||||
J = robot.getLinearJacobian(linkId, q)
|
||||
J = robot.get_linear_jacobian(linkId, q)
|
||||
|
||||
# get coriolis, gravity compensation torques
|
||||
torques = robot.get_coriolis_and_gravity_compensation_torques(q, dq)
|
||||
@@ -92,7 +100,7 @@ if __name__ == "__main__":
|
||||
F = K.dot(x_des - x) - D.dot(dx)
|
||||
# F = -D.dot(dx)
|
||||
tau = J.T.dot(F)
|
||||
print(tau)
|
||||
print("Torques: {}".format(tau))
|
||||
torques += tau
|
||||
robot.set_joint_torques(torques)
|
||||
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class KukaLWR(ManipulatorRobot):
|
||||
r"""Kuka LWR robot
|
||||
|
||||
@@ -7,6 +7,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.legged_robot import QuadrupedRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Laikago(QuadrupedRobot):
|
||||
r"""Laikago robot
|
||||
|
||||
@@ -5,10 +5,20 @@ Classes that are defined here: LeggedRobot, BipedRobot, QuadrupedRobot, HexapodR
|
||||
"""
|
||||
|
||||
import collections
|
||||
import itertools
|
||||
import numpy as np
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__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 LeggedRobot(Robot):
|
||||
r"""Legged robot
|
||||
@@ -17,12 +27,18 @@ class LeggedRobot(Robot):
|
||||
in the standard regime are rhythmic movements.
|
||||
"""
|
||||
|
||||
def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.):
|
||||
def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scaling=1.,
|
||||
foot_frictions=None):
|
||||
super(LeggedRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling=scaling)
|
||||
|
||||
# leg and feet ids
|
||||
self.legs = [] # list of legs where a leg is a list of links
|
||||
self.feet = [] # list of feet ids
|
||||
|
||||
# set the foot frictions
|
||||
if foot_frictions is not None:
|
||||
self.set_foot_friction(foot_frictions)
|
||||
|
||||
##############
|
||||
# Properties #
|
||||
##############
|
||||
|
||||
@@ -7,10 +7,22 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.legged_robot import QuadrupedRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class LittleDog(QuadrupedRobot):
|
||||
r"""Little Dog
|
||||
|
||||
References:
|
||||
[1] "The LittleDog Robot", Murphy et al., 2010
|
||||
https://journals.sagepub.com/doi/abs/10.1177/0278364910387457?journalCode=ijra
|
||||
[2] https://github.com/RobotLocomotion/LittleDog
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
@@ -4,6 +4,15 @@
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__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 ManipulatorRobot(Robot):
|
||||
r"""Manipulator robot
|
||||
|
||||
@@ -7,6 +7,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Manipulator2D(ManipulatorRobot):
|
||||
r"""2D manipulator robot
|
||||
|
||||
@@ -8,6 +8,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.legged_robot import QuadrupedRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Minitaur(QuadrupedRobot):
|
||||
r"""Minitaur robot
|
||||
@@ -229,7 +237,7 @@ if __name__ == "__main__":
|
||||
# print("Robot feet ids: {}".format(robot.feet))
|
||||
|
||||
# Position control using sliders
|
||||
# robot.add_joint_slider(robot.getLeftFrontLegIds())
|
||||
# robot.add_joint_slider(robot.left_front_leg)
|
||||
|
||||
t = 0
|
||||
# run simulator
|
||||
|
||||
@@ -7,6 +7,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.wheeled_robot import AckermannWheeledRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class MKZ(AckermannWheeledRobot):
|
||||
r"""Lincoln MKZ car
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.legged_robot import HexapodRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Morphex(HexapodRobot):
|
||||
r"""Morphex Hexapod robot
|
||||
|
||||
@@ -8,6 +8,14 @@ from pyrobolearn.robots.legged_robot import BipedRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
from pyrobolearn.robots.hand import TwoHand
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Nao(BipedRobot, BiManipulatorRobot, TwoHand):
|
||||
r"""Nao robot
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.legged_robot import QuadrupedRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class OpenDog(QuadrupedRobot):
|
||||
r""" OpenDog robot
|
||||
@@ -64,7 +72,7 @@ if __name__ == "__main__":
|
||||
robot.print_info()
|
||||
|
||||
# Position control using sliders
|
||||
# robot.add_joint_slider(robot.getLeftFrontLegIds())
|
||||
# robot.add_joint_slider(robot.left_front_leg)
|
||||
|
||||
# run simulator
|
||||
for _ in count():
|
||||
|
||||
@@ -8,6 +8,14 @@ from pyrobolearn.robots.wheeled_robot import WheeledRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
from pyrobolearn.robots.sensors.camera import CameraSensor
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Pepper(WheeledRobot, BiManipulatorRobot):
|
||||
r"""Pepper robot.
|
||||
@@ -38,7 +46,7 @@ class Pepper(WheeledRobot, BiManipulatorRobot):
|
||||
if fixed_base is None:
|
||||
fixed_base = False
|
||||
|
||||
super(Pepper, self).__init__(simulator, urdf, position, orientation, fixed_base)
|
||||
super(Pepper, self).__init__(simulator, urdf, position, orientation, fixed_base, scaling=scaling)
|
||||
self.name = 'pepper'
|
||||
|
||||
# 2D Camera sensor
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.legged_robot import HexapodRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class PhantomX(HexapodRobot):
|
||||
r"""Phantom X Hexapod robot
|
||||
|
||||
@@ -8,6 +8,14 @@ from pyrobolearn.robots.legged_robot import QuadrupedRobot
|
||||
from pyrobolearn.robots.uuv import UUVRobot
|
||||
from pyrobolearn.robots.usv import USVRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Pleurobot(QuadrupedRobot, UUVRobot, USVRobot):
|
||||
r"""Pleurobot Salamander robot
|
||||
|
||||
@@ -7,10 +7,21 @@ import os
|
||||
from pyrobolearn.robots.wheeled_robot import WheeledRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class PR2(WheeledRobot, BiManipulatorRobot):
|
||||
r"""PR2 robot
|
||||
|
||||
References:
|
||||
[1] http://www.willowgarage.com/pages/pr2/overview
|
||||
[2] https://github.com/pr2/pr2_common
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
@@ -6,6 +6,16 @@ import os
|
||||
import numpy as np
|
||||
|
||||
from pyrobolearn.robots.uav import RotaryWingUAV
|
||||
from pyrobolearn.utils.orientation import get_matrix_from_quaternion
|
||||
from pyrobolearn.utils.units import inches_to_meters, rpm_to_rad_per_second
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Quadcopter(RotaryWingUAV):
|
||||
@@ -81,7 +91,7 @@ class Quadcopter(RotaryWingUAV):
|
||||
self.diameter = 2. * self.radius
|
||||
self.area = np.pi * self.radius**2
|
||||
|
||||
self.max_velocity = 770 # rad/sec
|
||||
self.max_velocity = 770 # rad/sec
|
||||
|
||||
# joints 1 and 3 are CCW, and joints 2 and 4 are CW
|
||||
# CCW = +1, CW = -1
|
||||
@@ -95,26 +105,14 @@ class Quadcopter(RotaryWingUAV):
|
||||
# (in inches) and `P` is the propeller pitch (also in inches).
|
||||
# 1 inch = 0.0254m --> 0.28m = 11 inches
|
||||
# The value of the pitch below has been set by looking online for quadcopter props with 11 inches of length
|
||||
self.propeller_pitch = self.inchesToMeters(4.7) # self.inchesToMeters(5.)
|
||||
self.propeller_pitch = inches_to_meters(4.7) # inches_to_meters(5.)
|
||||
|
||||
# some constants
|
||||
self.k1 = 1./3.29546
|
||||
self.k2 = 1.5
|
||||
|
||||
def inchesToMeters(self, inch):
|
||||
return inch * 0.0254
|
||||
|
||||
def metersToInches(self, meter):
|
||||
return meter / 0.0254
|
||||
|
||||
def rpmToRadPerSecond(self, rpm):
|
||||
return rpm * 2 * np.pi/60
|
||||
|
||||
def radPerSecondToRPM(self, omega):
|
||||
return omega * 60 / (2*np.pi)
|
||||
|
||||
def calculateThrustForce(self, angular_speed, area, propeller_pitch, v0=0, air_density=1.225):
|
||||
"""
|
||||
def calculate_thrust_force(self, angular_speed, area, propeller_pitch, v0=0, air_density=1.225):
|
||||
r"""
|
||||
Calculate the thrust force generated by the propeller (based on [6]).
|
||||
|
||||
Args:
|
||||
@@ -123,6 +121,7 @@ class Quadcopter(RotaryWingUAV):
|
||||
area (float): area of the propeller [m^2]
|
||||
propeller_pitch (float): "distance a propeller would move in 1 revolution if it were moving through a
|
||||
soft solid" [m]
|
||||
v0 (float): initial velocity
|
||||
air_density (float): density of air [kg/m^3]. By default, it is the density of the air at sea level and at
|
||||
15 degrees Celsius. Note that this varies with the temperature, humidity, and pressure. It decreases
|
||||
with increasing altitude.
|
||||
@@ -134,7 +133,7 @@ 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 set_joint_velocities(self, velocities, joint_ids=None, maxVelocity=True, maxTorque=True):
|
||||
def set_joint_velocities(self, velocities, joint_ids=None, max_velocity=True, forces=True):
|
||||
"""
|
||||
Set the joint velocities and apply the thrust force on the propeller link corresponding to the given
|
||||
joint id(s).
|
||||
@@ -142,8 +141,11 @@ class Quadcopter(RotaryWingUAV):
|
||||
Args:
|
||||
velocities (float[4]): velocity of each propeller
|
||||
joint_ids (int[4], None): Not used here
|
||||
maxVelocity (bool):
|
||||
maxTorque (bool):
|
||||
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.
|
||||
|
||||
Returns:
|
||||
None
|
||||
@@ -154,34 +156,34 @@ class Quadcopter(RotaryWingUAV):
|
||||
joint_ids = self.joints
|
||||
|
||||
# call parent method
|
||||
super(Quadcopter, self).set_joint_velocities(velocities, joint_ids, maxVelocity, maxTorque)
|
||||
super(Quadcopter, self).set_joint_velocities(velocities, joint_ids, max_velocity, forces)
|
||||
|
||||
# calculate thrust force of the given joints, and apply it on the link
|
||||
for jnt, d, v in zip(joint_ids, self.turning_directions, velocities):
|
||||
if maxVelocity and v > self.max_velocity:
|
||||
if max_velocity and v > self.max_velocity:
|
||||
v = self.max_velocity
|
||||
|
||||
# compute propeller speed v0
|
||||
state = self.sim.getLinkState(self.id, jnt, computeLinkVelocity=True) #, computeForwardKinematics=True)
|
||||
R = np.array(self.sim.getMatrixFromQuaternion(state[1])).reshape(3, 3)
|
||||
state = self.sim.get_link_state(self.id, jnt, compute_velocity=True) # , compute_forward_kinematics=True)
|
||||
R = np.array(get_matrix_from_quaternion(state[1]))
|
||||
linear_velocity = np.array(state[-2])
|
||||
propeller_upVec = R.dot(np.array([0.,0.,1.]))
|
||||
v0 = linear_velocity.dot(propeller_upVec)
|
||||
propeller_up_vec = R.dot(np.array([0., 0., 1.]))
|
||||
v0 = linear_velocity.dot(propeller_up_vec)
|
||||
# v0 = 0 # static thrust
|
||||
|
||||
# compute thrust
|
||||
f = self.calculateThrustForce(v*d, self.area, self.propeller_pitch, v0)
|
||||
f = self.calculate_thrust_force(v * d, self.area, self.propeller_pitch, v0)
|
||||
# f = self.mass * self.gravity / 4.
|
||||
|
||||
# apply force in the simulation
|
||||
self.apply_external_force([0, 0, f], jnt, position=(0., 0., 0.))
|
||||
|
||||
def getStationaryJointVelocity(self):
|
||||
def get_stationary_joint_velocity(self):
|
||||
fg = self.mass * self.gravity / 4.
|
||||
p = self.propeller_pitch
|
||||
return (2*np.pi / p) * (fg / (self.air_density * self.area) * (p / (self.k1 * self.diameter))**self.k2)**0.5
|
||||
|
||||
def getStationaryRPM(self):
|
||||
def get_stationary_rpm(self):
|
||||
fg = self.mass * self.gravity / 4.
|
||||
p = self.propeller_pitch
|
||||
return (60 / p) * (fg / (self.air_density * self.area) * (p / (self.k1 * self.diameter))**self.k2)**0.5
|
||||
@@ -192,7 +194,6 @@ class Quadcopter(RotaryWingUAV):
|
||||
|
||||
# Test
|
||||
if __name__ == "__main__":
|
||||
import numpy as np
|
||||
from itertools import count
|
||||
from pyrobolearn.simulators import BulletSim
|
||||
from pyrobolearn.worlds import BasicWorld
|
||||
@@ -209,9 +210,9 @@ if __name__ == "__main__":
|
||||
# print information about the robot
|
||||
robot.print_info()
|
||||
|
||||
rpm = robot.getStationaryRPM()
|
||||
rpm = robot.get_stationary_rpm()
|
||||
print("Stationary RPM: {}".format(rpm))
|
||||
v = robot.rpmToRadPerSecond(rpm+20)
|
||||
v = rpm_to_rad_per_second(rpm + 20)
|
||||
v = [v, -v, v, -v]
|
||||
|
||||
# run simulation
|
||||
|
||||
@@ -7,6 +7,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.legged_robot import HexapodRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
# TODO add inertial tags
|
||||
class Rhex(HexapodRobot):
|
||||
|
||||
@@ -1650,10 +1650,10 @@ class Robot(ControllableBody):
|
||||
|
||||
Args:
|
||||
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.
|
||||
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).
|
||||
q (np.float[N], None): joint positions of size N, where N is the number of DoFs. If None, it will compute q
|
||||
based on the current joint positions.
|
||||
local_position (None, np.array[3]): 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:
|
||||
np.float[6,N], np.float[6,(6+N)]: full geometric (linear and angular) Jacobian matrix. The number of columns
|
||||
|
||||
+20
-13
@@ -7,6 +7,14 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class RRBot(ManipulatorRobot):
|
||||
r"""RRBot
|
||||
@@ -147,19 +155,19 @@ if __name__ == "__main__":
|
||||
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, frame=1) # link_frame = 1, world_frame = 2
|
||||
|
||||
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.get_jacobian(robot.id, 1, [0., 0., 0.])
|
||||
J = sim.calculate_jacobian(robot.id, 1, [0., 0., 0.])
|
||||
print(np.array(J[0]))
|
||||
|
||||
a = robot.get_joint_positions()
|
||||
# print(robot.getJacobianMatrix(1, np.array([0.,0.]))) # TODO: need to convert numpy array to list
|
||||
# print(robot.get_jacobian(1, np.array([0.,0.]))) # TODO: need to convert numpy array to list
|
||||
|
||||
linkId = 2
|
||||
com_frame = robot.getLinkStates(linkId)[2]
|
||||
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
|
||||
@@ -176,11 +184,11 @@ if __name__ == "__main__":
|
||||
# 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.get_jacobian(robot.id, linkId, com_frame)[0])
|
||||
F = K.dot(xdes - x) - D.dot(dx) # compute cartesian forces
|
||||
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)
|
||||
@@ -191,14 +199,13 @@ if __name__ == "__main__":
|
||||
# print("manipulability: {}".format(w))
|
||||
|
||||
# Impedance/Torque control
|
||||
sim.setJointMotorControlArray(robot.id, robot.joint_indices, sim.TORQUE_CONTROL, forces=tau)
|
||||
|
||||
force = sim.readUserDebugParameter(slider)
|
||||
sim.set_joint_motor_control(robot.id, robot.joints, sim.TORQUE_CONTROL, forces=tau)
|
||||
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, frame=1) # p.LINK_FRAME = 1, p.WORLD_FRAME = 2
|
||||
|
||||
# robot.update_joint_slider()
|
||||
world.step(sleep_dt=1./240)
|
||||
|
||||
@@ -7,6 +7,14 @@ import os
|
||||
from pyrobolearn.robots.wheeled_robot import WheeledRobot
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Sawyer(ManipulatorRobot, WheeledRobot):
|
||||
r"""Sawyer robot
|
||||
@@ -15,6 +23,7 @@ class Sawyer(ManipulatorRobot, WheeledRobot):
|
||||
|
||||
References:
|
||||
[1] Rethink Robotics
|
||||
[2] https://github.com/RethinkRobotics/sawyer_robot
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.legged_robot import HexapodRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class SEAHexapod(HexapodRobot):
|
||||
r"""SEA Hexapod robot (from CMU Biorobotics Lab)
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class SEASnake(Robot):
|
||||
r"""SEA snake robot (from CMU Biorobotics Lab)
|
||||
|
||||
@@ -0,0 +1,87 @@
|
||||
#!/usr/bin/env python
|
||||
"""Provide the Shadow Hand robotic platform.
|
||||
"""
|
||||
|
||||
import os
|
||||
|
||||
from pyrobolearn.robots.hand import Hand
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class ShadowHand(Hand):
|
||||
r"""Shadow Hand
|
||||
|
||||
References:
|
||||
[1] https://www.shadowrobot.com/products/dexterous-hand/
|
||||
[2] Shadow hand description: https://github.com/shadow-robot/sr_common
|
||||
[3] Documentation: https://dexterous-hand.readthedocs.io/en/latest/index.html
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
simulator,
|
||||
position=(0, 0, 0),
|
||||
orientation=(0, 0, 0.707, 0.707),
|
||||
scaling=1.,
|
||||
left=True,
|
||||
fixed_base=True):
|
||||
# check parameters
|
||||
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 orientation is None:
|
||||
orientation = (0, 0, 0.707, 0.707)
|
||||
urdf_path = os.path.dirname(__file__) + '/urdfs/shadowhand/left_hand.urdf'
|
||||
else:
|
||||
if orientation is None:
|
||||
orientation = (0, 0, 1, 0)
|
||||
urdf_path = os.path.dirname(__file__) + '/urdfs/shadowhand/right_hand.urdf'
|
||||
|
||||
super(ShadowHand, self).__init__(simulator, urdf_path, position, orientation, fixed_base, scaling)
|
||||
self.name = 'shadow_hand'
|
||||
|
||||
|
||||
# Test
|
||||
if __name__ == "__main__":
|
||||
from itertools import count
|
||||
from pyrobolearn.simulators import BulletSim
|
||||
from pyrobolearn.worlds import BasicWorld
|
||||
|
||||
# Create simulator
|
||||
sim = BulletSim()
|
||||
|
||||
# create world
|
||||
world = BasicWorld(sim)
|
||||
|
||||
# create robot
|
||||
left_hand = ShadowHand(sim, position=(-0.15, 0, 0), left=True)
|
||||
right_hand = ShadowHand(sim, position=(0.15, 0., 0.), orientation=(0, 0, 0.707, -0.707), left=False)
|
||||
|
||||
# print information about the robot
|
||||
left_hand.print_info()
|
||||
# H = left_hand.get_mass_matrix()
|
||||
# print("Inertia matrix: H(q) = {}".format(H))
|
||||
|
||||
# Position control using sliders
|
||||
# left_hand.add_joint_slider()
|
||||
|
||||
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.update_joint_slider()
|
||||
# left_hand.set_joint_positions([0.] * left_hand.num_dofs)
|
||||
# right_hand.set_joint_positions([0.] * right_hand.num_dofs)
|
||||
|
||||
world.step(sleep_dt=1./240)
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.hand import Hand
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
# TODO: correct the inertia matrices and masses; they are too big!
|
||||
class SoftHand(Hand):
|
||||
@@ -68,12 +76,12 @@ if __name__ == "__main__":
|
||||
# Position control using sliders
|
||||
# left_hand.add_joint_slider()
|
||||
|
||||
left_hand.set_joint_positions([0.] * left_hand.getNumberOfDoFs())
|
||||
right_hand.set_joint_positions([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.update_joint_slider()
|
||||
# left_hand.set_joint_positions([0.] * left_hand.getNumberOfDoFs())
|
||||
# right_hand.set_joint_positions([0.] * right_hand.getNumberOfDoFs())
|
||||
# left_hand.set_joint_positions([0.] * left_hand.num_dofs)
|
||||
# right_hand.set_joint_positions([0.] * right_hand.num_dofs)
|
||||
|
||||
world.step(sleep_dt=1./240)
|
||||
|
||||
@@ -6,9 +6,20 @@ import os
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Swimmer(Robot):
|
||||
r"""Swimmer Mujoco Model
|
||||
|
||||
References:
|
||||
[1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
@@ -4,6 +4,15 @@
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__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 UAVRobot(Robot):
|
||||
r"""Unmanned Aerial Vehicle Robot
|
||||
|
||||
@@ -6,6 +6,10 @@ Here is the list of repos where you can find the original URDF/meshes of each ro
|
||||
* Pepper: https://github.com/ros-naoqi/pepper_robot
|
||||
* PR2: https://github.com/pr2/pr2_common
|
||||
* Fetch: https://github.com/fetchrobotics/fetch_ros
|
||||
* Youbot: https://github.com/youbot/youbot_description
|
||||
* Epuck: https://github.com/gctronic/epuck_driver_cpp
|
||||
* F10 racecar: https://github.com/erwincoumans/pybullet_robots/tree/master/data/f10_racecar
|
||||
* Lincoln MKZ car: https://bitbucket.org/DataspeedInc/dbw_mkz_ros
|
||||
|
||||
* Quadruped
|
||||
* Aibo: https://github.com/dkotfis/aibo_ros
|
||||
@@ -23,6 +27,8 @@ Here is the list of repos where you can find the original URDF/meshes of each ro
|
||||
* Atlas: https://github.com/openai/roboschool
|
||||
* Valkyrie: https://github.com/openhumanoids/val_description
|
||||
* Cassie: https://github.com/UMich-BipedLab/Cassie_Model and https://github.com/agilityrobotics/cassie-gazebo-sim
|
||||
* Darwin: https://github.com/HumaRobotics/darwin_description
|
||||
* Blackbird: https://hackaday.io/project/160882-blackbird-bipedal-robot
|
||||
|
||||
* Manipulator
|
||||
* rrbot: https://github.com/ros-simulation/gazebo_ros_demos
|
||||
@@ -31,11 +37,18 @@ Here is the list of repos where you can find the original URDF/meshes of each ro
|
||||
* franka-emika: https://github.com/frankaemika/franka_ros
|
||||
* Baxter: https://github.com/RethinkRobotics/baxter_common
|
||||
* Jaco: https://github.com/JenniferBuehler/jaco-arm-pkgs
|
||||
* e.DO: https://github.com/Comau/eDO_description
|
||||
|
||||
* Hexapod
|
||||
* phantomx: https://github.com/HumaRobotics/phantomx_description
|
||||
* crab: https://github.com/tuuzdu/crab_project
|
||||
* SEA (Series Elastic Actuators) hexapod (CMU): https://github.com/alexansari101/snake_ws
|
||||
* Rhex: https://github.com/grafoteka/rhex
|
||||
|
||||
* Hand
|
||||
* allegrohand: https://github.com/simlabrobotics/allegro_hand_ros
|
||||
* softhand: https://github.com/CentroEPiaggio/pisa-iit-soft-hand
|
||||
* shadowhand: https://github.com/shadow-robot/sr_common and https://github.com/shadow-robot/sr_ur_arm
|
||||
|
||||
* UVA
|
||||
* quadcopter: https://github.com/wilselby/ROS_quadrotor_simulator
|
||||
@@ -52,7 +65,8 @@ Here is the list of repos where you can find the original URDF/meshes of each ro
|
||||
* SEA (Series Elastic Actuators) snake (CMU): https://github.com/alexansari101/snake_ws
|
||||
|
||||
* Others
|
||||
* cubli: modeled by Songyan Xin
|
||||
* cubli: modeled by Songyan Xin
|
||||
* ballbot: https://github.com/CesMak/bb
|
||||
|
||||
|
||||
**Notes**:
|
||||
|
||||
@@ -0,0 +1,25 @@
|
||||
BSD 2-Clause License
|
||||
|
||||
Copyright (c) 2019, The Shadow Robot Company
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
* Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@@ -0,0 +1,115 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<COLLADA version="1.4.0" xmlns="http://www.collada.org/2005/11/COLLADASchema">
|
||||
<asset>
|
||||
<contributor>
|
||||
<author>Illusoft Collada 1.4.0 plugin for Blender - http://colladablender.illusoft.com</author>
|
||||
<authoring_tool>Blender v:249 - Illusoft Collada Exporter v:0.3.162</authoring_tool>
|
||||
<comments></comments>
|
||||
<copyright></copyright>
|
||||
<source_data>file:///media/ce7ff5ed-1b73-453e-869e-58fb97d47a68/ProjetHandle/ShadowHand/Modeles/VALIDE_vOptimizedWithoutDetails/BETA_shadow_hand_2012-07-22_envelop_opt_Zaligned_texture_decimate-ready.blend</source_data>
|
||||
</contributor>
|
||||
<created>2012-07-23T02:13:47.090860</created>
|
||||
<modified>2012-07-23T02:13:47.090873</modified>
|
||||
<unit meter="1.0" name="meter"/>
|
||||
<up_axis>Y_UP</up_axis>
|
||||
</asset>
|
||||
<library_effects>
|
||||
<effect id="black_spec-fx" name="black_spec-fx">
|
||||
<profile_COMMON>
|
||||
<technique sid="blender">
|
||||
<phong>
|
||||
<emission>
|
||||
<color>0.00000 0.00000 0.00000 1</color>
|
||||
</emission>
|
||||
<ambient>
|
||||
<color>0.05997 0.05997 0.05997 1</color>
|
||||
</ambient>
|
||||
<diffuse>
|
||||
<color>0.16355 0.16355 0.16355 1</color>
|
||||
</diffuse>
|
||||
<specular>
|
||||
<color>0.36585 0.36585 0.36585 1</color>
|
||||
</specular>
|
||||
<shininess>
|
||||
<float>12.5</float>
|
||||
</shininess>
|
||||
<reflective>
|
||||
<color>1.00000 1.00000 1.00000 1</color>
|
||||
</reflective>
|
||||
<reflectivity>
|
||||
<float>0.0</float>
|
||||
</reflectivity>
|
||||
<transparent>
|
||||
<color>1 1 1 1</color>
|
||||
</transparent>
|
||||
<transparency>
|
||||
<float>0.0</float>
|
||||
</transparency>
|
||||
</phong>
|
||||
</technique>
|
||||
</profile_COMMON>
|
||||
</effect>
|
||||
</library_effects>
|
||||
<library_materials>
|
||||
<material id="black_spec" name="black_spec">
|
||||
<instance_effect url="#black_spec-fx"/>
|
||||
</material>
|
||||
</library_materials>
|
||||
<library_geometries>
|
||||
<geometry id="th_distal_001" name="th_distal_001">
|
||||
<mesh>
|
||||
<source id="th_distal_001-Position">
|
||||
<float_array count="177" id="th_distal_001-Position-array">-1.94990 -7.25614 29.02264 3.33611 -4.68227 32.81317 1.94990 -7.25538 29.02197 -3.87013 -8.65177 22.59167 3.86920 -8.64894 22.60528 -2.83502 -9.64354 15.65996 2.83500 -9.64354 15.65996 -3.93947 -9.70870 11.08414 3.85520 -9.71829 11.09277 -4.68515 8.86393 0.55981 -2.85362 8.40625 15.03454 3.38880 8.97677 0.56587 2.85360 8.40625 15.03454 -2.61370 8.19168 19.12334 2.61370 8.19168 19.12334 -3.60638 7.56873 27.43241 3.61120 7.56791 27.43315 10.58510 1.11213 10.90711 10.52110 -4.02011 4.70263 10.73880 -2.78497 3.28595 10.89100 -0.01093 0.09471 10.44900 -2.27405 13.40703 10.09550 0.67434 19.25264 9.73710 -2.88044 22.00933 9.23020 1.90563 27.65761 -10.34030 0.89364 15.08018 -9.81313 -1.24062 22.92131 -8.76240 2.29487 29.29462 -10.30403 -2.11973 15.79719 -10.53455 -3.97056 4.65800 -10.87858 -0.11526 0.18446 -7.93252 7.03247 15.28926 -7.55426 7.70909 0.60254 -7.55883 5.44594 28.79698 -9.99739 3.20872 15.18097 -6.63448 -5.04469 28.84449 -9.72552 -5.02092 15.96375 -9.14490 -6.63237 7.59936 -6.96709 -8.36411 16.02057 -7.15805 -7.82629 22.75654 -6.32660 -9.01327 10.45796 -7.65099 1.70142 31.55542 -6.14634 -2.16702 32.46600 -2.89731 -0.63313 34.85239 -4.14867 -4.40515 32.73298 5.89542 -3.36571 32.30459 7.62490 -0.96751 31.62656 3.08331 -0.59315 34.94931 6.52960 -5.63257 28.95620 9.72480 -5.02010 15.97646 9.03180 -6.77846 7.73091 6.96630 -8.36318 16.03400 6.53980 -7.53452 22.60703 6.11650 -9.11016 10.54522 8.47849 7.00809 15.35375 7.99230 7.39971 0.48279 7.78820 4.91574 27.68161 10.14380 3.24984 15.18304 0.00000 5.24556 33.40231</float_array>
|
||||
<technique_common>
|
||||
<accessor count="59" source="#th_distal_001-Position-array" stride="3">
|
||||
<param type="float" name="X"></param>
|
||||
<param type="float" name="Y"></param>
|
||||
<param type="float" name="Z"></param>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<source id="th_distal_001-Normals">
|
||||
<float_array count="339" id="th_distal_001-Normals-array">0.00026 -0.82747 0.56151 0.00023 -0.97727 0.21202 -0.00002 -0.97722 0.21223 0.00011 -0.98992 0.14165 -0.00000 -0.98990 0.14176 -0.00000 -0.99990 0.01424 -0.00125 -0.99987 0.01609 -0.01399 0.99935 0.03337 -0.00000 0.99922 0.03940 -0.00000 0.99863 0.05241 0.00000 0.99863 0.05241 0.00000 0.99720 0.07476 0.00011 0.99720 0.07484 0.99300 -0.09584 0.06904 0.99934 -0.01992 0.03034 0.99946 -0.02967 0.01423 0.99827 0.00324 0.05873 0.99607 -0.03861 0.07971 0.99428 -0.01874 0.10511 -0.98830 0.11669 0.09821 -0.99766 0.00422 0.06822 -0.99974 -0.00684 0.02183 -0.99828 -0.04378 0.03904 -0.24269 0.96912 0.04366 -0.25747 0.96405 0.06570 -0.25764 0.96418 0.06309 -0.37277 0.92733 0.03312 -0.43832 0.89120 0.11680 -0.92832 0.36541 0.06860 -0.92025 0.39129 -0.00567 -0.87799 0.47565 -0.05384 -0.87610 0.47007 0.10710 -0.98904 0.14537 0.02589 -0.98529 0.14179 0.09540 -0.90903 -0.28235 0.30650 -0.97992 -0.19240 0.05225 -0.97763 -0.18973 0.09078 -0.89645 -0.44255 0.02303 -0.77009 -0.63422 0.06873 -0.89701 -0.36777 0.24518 -0.65305 -0.75720 0.01317 -0.40225 -0.81916 0.40887 -0.22175 -0.93710 0.26958 -0.23957 -0.96831 0.07052 -0.29739 -0.95166 0.07682 -0.28813 -0.95398 0.08313 -0.28702 -0.95335 0.09354 -0.58097 -0.03456 0.81319 -0.46173 -0.31313 0.82991 -0.89187 -0.25975 0.37026 -0.89600 -0.28087 0.34395 -0.66885 -0.53548 0.51565 -0.40452 -0.82515 0.39432 -0.78783 0.37774 0.48645 0.57030 -0.18500 0.80033 -0.01090 -0.48707 0.87330 -0.02667 -0.46415 0.88535 0.37627 -0.41061 0.83055 -0.03610 -0.80265 0.59536 -0.04375 0.07183 -0.99646 0.91012 -0.27224 0.31236 0.97847 -0.19515 0.06722 0.97632 -0.21042 0.05030 0.88979 -0.45581 0.02242 0.75461 -0.61891 0.21797 0.77000 -0.63414 0.07052 0.82276 -0.54412 0.16432 0.82528 -0.55116 0.12303 0.62945 -0.77699 0.00829 0.32489 -0.90583 0.27187 0.38124 -0.91382 0.13994 0.36709 -0.88016 0.30095 0.27636 -0.95793 0.07741 0.28790 -0.95386 0.08524 0.28696 -0.95334 0.09379 0.32429 0.94585 0.01431 0.23844 0.97002 0.04707 0.52084 0.83633 0.17111 0.89541 0.42797 0.12278 0.93072 0.36514 -0.02081 0.91272 0.40637 -0.04262 0.91059 0.39851 0.10963 0.98965 0.11566 0.08494 0.99481 0.08021 0.06257 0.98254 0.18584 0.00850 0.42778 0.72438 0.54063 0.54565 0.47167 0.69267 0.00007 0.93194 0.36262 -0.01735 0.24751 0.96873 -0.38447 0.39527 0.83423 -0.27581 0.82770 0.48871 -0.42626 0.54330 0.72327 0.74917 0.35443 0.55958 0.53852 0.45891 0.70668 0.93816 -0.30930 0.15554 0.78147 -0.43790 0.44447 0.46508 -0.68969 0.55501 0.30292 -0.83636 0.45688 0.00017 0.04165 -0.99913 -0.00239 0.04565 -0.99895 -0.00436 0.05060 -0.99871 0.07488 -0.75527 -0.65112 -0.05396 -0.74074 -0.66962 0.04993 -0.76443 -0.64277 -0.00019 -0.74967 -0.66181 -0.03215 -0.72749 -0.68537 0.01032 -0.76332 -0.64594 -0.00535 -0.74271 -0.66959 0.00094 -0.75747 -0.65286 0.23726 0.96928 0.06479 0.22590 0.97307 0.04593 -0.77126 -0.63587 0.02890 -0.90887 -0.37118 0.19023</float_array>
|
||||
<technique_common>
|
||||
<accessor count="113" source="#th_distal_001-Normals-array" stride="3">
|
||||
<param type="float" name="X"></param>
|
||||
<param type="float" name="Y"></param>
|
||||
<param type="float" name="Z"></param>
|
||||
</accessor>
|
||||
</technique_common>
|
||||
</source>
|
||||
<vertices id="th_distal_001-Vertex">
|
||||
<input semantic="POSITION" source="#th_distal_001-Position"/>
|
||||
</vertices>
|
||||
<triangles count="113" material="black_spec">
|
||||
<input offset="0" semantic="VERTEX" source="#th_distal_001-Vertex"/>
|
||||
<input offset="1" semantic="NORMAL" source="#th_distal_001-Normals"/>
|
||||
<p>1 0 0 0 2 0 0 1 3 1 2 1 4 2 2 2 3 2 3 3 5 3 4 3 6 4 4 4 5 4 5 5 7 5 6 5 8 6 6 6 7 6 9 7 10 7 11 7 12 8 11 8 10 8 10 9 13 9 12 9 14 10 12 10 13 10 13 11 15 11 14 11 16 12 14 12 15 12 17 13 18 13 19 13 20 14 17 14 19 14 21 15 18 15 17 15 17 16 22 16 21 16 23 17 21 17 22 17 22 18 24 18 23 18 25 19 26 19 27 19 28 20 26 20 25 20 29 21 28 21 25 21 25 22 30 22 29 22 31 23 15 23 13 23 10 24 31 24 13 24 31 25 10 25 9 25 9 26 32 26 31 26 33 27 15 27 31 27 27 28 33 28 31 28 31 29 32 29 30 29 34 30 31 30 30 30 31 31 34 31 27 31 34 32 30 32 25 32 34 33 25 33 27 33 35 34 27 34 26 34 36 35 28 35 29 35 28 36 36 36 26 36 29 37 37 37 36 37 38 38 36 38 37 38 26 39 39 39 35 39 37 40 40 40 38 40 0 41 35 41 39 41 3 42 0 42 39 42 3 43 39 43 38 43 38 44 40 44 7 44 5 45 38 45 7 45 38 46 5 46 3 46 41 47 42 47 43 47 44 48 43 48 42 48 27 49 42 49 41 49 42 50 27 50 35 50 44 51 42 51 35 51 35 52 0 52 44 52 33 53 27 53 41 53 45 54 46 54 47 54 47 55 43 55 44 55 1 56 47 56 44 56 47 57 1 57 45 57 44 58 0 58 1 58 32 59 9 59 30 59 24 60 48 60 23 60 49 61 21 61 23 61 21 62 49 62 18 62 50 63 18 63 49 63 51 64 49 64 23 64 49 65 51 65 50 65 52 66 23 66 48 66 23 67 52 67 51 67 51 68 53 68 50 68 48 69 2 69 52 69 52 70 4 70 51 70 4 71 52 71 2 71 6 72 8 72 53 72 51 73 6 73 53 73 6 74 51 74 4 74 54 75 55 75 11 75 54 76 11 76 12 76 16 77 56 77 54 77 56 78 24 78 54 78 20 79 55 79 54 79 54 80 57 80 20 80 57 81 54 81 24 81 57 82 24 82 22 82 17 83 57 83 22 83 57 84 17 84 20 84 58 85 56 85 16 85 47 86 46 86 58 86 16 87 15 87 58 87 43 88 47 88 58 88 41 89 43 89 58 89 58 90 15 90 33 90 58 91 33 91 41 91 24 92 56 92 46 92 58 93 46 93 56 93 48 94 24 94 46 94 46 95 45 95 48 95 45 96 1 96 48 96 1 97 2 97 48 97 30 98 9 98 11 98 30 99 11 99 55 99 30 100 55 100 20 100 20 101 18 101 50 101 20 102 50 102 53 102 20 103 53 103 8 103 20 104 8 104 7 104 20 105 7 105 40 105 20 106 40 106 37 106 20 107 37 107 29 107 20 108 29 108 30 108 14 109 54 109 12 109 14 110 16 110 54 110 36 111 38 111 39 111 26 112 36 112 39 112</p>
|
||||
</triangles>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</library_geometries>
|
||||
<library_visual_scenes>
|
||||
<visual_scene id="Scene" name="Scene">
|
||||
<node layer="L9" id="th_distal" name="th_distal">
|
||||
<translate sid="translate">0.00000 0.00000 0.00000</translate>
|
||||
<rotate sid="rotateZ">0 0 1 0.00000</rotate>
|
||||
<rotate sid="rotateY">0 1 0 -0.00000</rotate>
|
||||
<rotate sid="rotateX">1 0 0 0.00000</rotate>
|
||||
<scale sid="scale">1.00000 1.00000 1.00000</scale>
|
||||
<instance_geometry url="#th_distal_001">
|
||||
<bind_material>
|
||||
<technique_common>
|
||||
<instance_material symbol="black_spec" target="#black_spec">
|
||||
<bind_vertex_input input_semantic="TEXCOORD" input_set="1" semantic="CHANNEL1"/>
|
||||
</instance_material>
|
||||
</technique_common>
|
||||
</bind_material>
|
||||
</instance_geometry>
|
||||
</node>
|
||||
</visual_scene>
|
||||
</library_visual_scenes>
|
||||
<scene>
|
||||
<instance_visual_scene url="#Scene"/>
|
||||
</scene>
|
||||
</COLLADA>
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 25 KiB |
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
@@ -4,6 +4,15 @@
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__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 USVRobot(Robot):
|
||||
r"""Unmanned Surface Vehicle Robot
|
||||
|
||||
@@ -4,6 +4,15 @@
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__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 UUVRobot(Robot):
|
||||
r"""Unmanned Underwater Vehicle Robot
|
||||
|
||||
@@ -6,9 +6,20 @@ import os
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Walker2D(Robot):
|
||||
r"""Walker 2D Mujoco Model
|
||||
|
||||
References:
|
||||
[1] description: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/mjcf
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
|
||||
@@ -8,6 +8,14 @@ from pyrobolearn.robots.legged_robot import BipedRobot
|
||||
from pyrobolearn.robots.manipulator import BiManipulatorRobot
|
||||
from pyrobolearn.robots.sensors import CameraSensor
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class Walkman(BipedRobot, BiManipulatorRobot):
|
||||
r"""Walk-man robot
|
||||
@@ -18,6 +26,7 @@ class Walkman(BipedRobot, BiManipulatorRobot):
|
||||
|
||||
References:
|
||||
[1] "WALK-MAN: A High-Performance Humanoid Platform for Realistic Environments", Tsagarakis et al., 2017
|
||||
[2] https://github.com/ADVRHumanoids/iit-walkman-ros-pkg
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
@@ -50,7 +59,7 @@ class Walkman(BipedRobot, BiManipulatorRobot):
|
||||
self.left_camera = CameraSensor(self.sim, self.id, 11, width=800, height=800, fovy=80, near=0.02, far=300,
|
||||
refresh_rate=30) # 11
|
||||
self.right_camera = CameraSensor(self.sim, self.id, 13, width=800, height=800, fovy=80, near=0.02, far=300,
|
||||
refresh_rate=30) # 13
|
||||
refresh_rate=30) # 13
|
||||
|
||||
# Laser (depth) sensor: Hokuyo sensor
|
||||
# link: "head_hokuyo_frame"
|
||||
@@ -82,13 +91,6 @@ class Walkman(BipedRobot, BiManipulatorRobot):
|
||||
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, position=init_pos, orientation=init_orient, fixed_base=useFixedBase,
|
||||
scaling=scaling, urdf=urdf_path)
|
||||
|
||||
|
||||
# Test
|
||||
if __name__ == "__main__":
|
||||
# Imports
|
||||
@@ -101,8 +103,8 @@ if __name__ == "__main__":
|
||||
|
||||
# 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, fixed_base=False, lower_body=False)
|
||||
|
||||
@@ -6,6 +6,14 @@ import os
|
||||
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class WAM(ManipulatorRobot):
|
||||
r"""Wam robot
|
||||
@@ -58,7 +66,7 @@ if __name__ == "__main__":
|
||||
# H = robot.get_mass_matrix()
|
||||
# print("Inertia matrix: H(q) = {}".format(H))
|
||||
|
||||
robot.set_joint_positions([np.pi / 4, np.pi / 2], joint_ids=[0, 1]) #2, 4])
|
||||
robot.set_joint_positions([np.pi / 4, np.pi / 2], joint_ids=[0, 1]) # 2, 4])
|
||||
|
||||
Jlin = robot.get_jacobian(6)[:3]
|
||||
robot.draw_velocity_manipulability_ellipsoid(6, Jlin, color=(1, 0, 0, 0.7))
|
||||
|
||||
@@ -6,6 +6,15 @@ import numpy as np
|
||||
|
||||
from pyrobolearn.robots.robot import Robot
|
||||
|
||||
__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 WheeledRobot(Robot):
|
||||
r"""Wheeled robot
|
||||
|
||||
@@ -10,6 +10,14 @@ import numpy as np
|
||||
from pyrobolearn.robots.manipulator import ManipulatorRobot, BiManipulatorRobot
|
||||
from pyrobolearn.robots.wheeled_robot import DifferentialWheeledRobot
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__license__ = "MIT"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class YoubotBase(DifferentialWheeledRobot):
|
||||
r"""Youbot Base robot
|
||||
|
||||
@@ -1272,7 +1272,7 @@ class Bullet(Simulator):
|
||||
|
||||
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):
|
||||
"""
|
||||
r"""
|
||||
Set the joint motor control.
|
||||
|
||||
In position control:
|
||||
@@ -1294,7 +1294,7 @@ class Bullet(Simulator):
|
||||
|
||||
Args:
|
||||
body_id (int): body unique id.
|
||||
joint_ids (int): joint/link id, or list of joint ids.
|
||||
joint_ids ((list of) 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).
|
||||
positions (float, np.float[N]): target joint position(s) (used in POSITION_CONTROL).
|
||||
@@ -1865,6 +1865,8 @@ class Bullet(Simulator):
|
||||
joint_ids (int, list of int): joint id, or list of joint ids.
|
||||
accelerations (float, np.float[N]): desired joint acceleration, or list of desired joint accelerations
|
||||
[rad/s^2]
|
||||
q (None, list of float, float): current joint positions.
|
||||
dq (None, list of float, float): current joint velocities.
|
||||
"""
|
||||
# check joint ids
|
||||
if isinstance(joint_ids, int):
|
||||
@@ -2935,7 +2937,7 @@ class Bullet(Simulator):
|
||||
self.sim.changeDynamics(body_id, link_id, **kwargs)
|
||||
|
||||
def calculate_jacobian(self, body_id, link_id, local_position, q, dq, des_ddq):
|
||||
"""
|
||||
r"""
|
||||
Return the full geometric Jacobian matrix :math:`J(q) = [J_{lin}(q), J_{ang}(q)]^T`, such that:
|
||||
|
||||
.. math:: v = [\dot{p}, \omega]^T = J(q) \dot{q}
|
||||
@@ -2975,7 +2977,7 @@ class Bullet(Simulator):
|
||||
return np.vstack((lin_jac, ang_jac))
|
||||
|
||||
def calculate_mass_matrix(self, body_id, q):
|
||||
"""
|
||||
r"""
|
||||
Return the mass/inertia matrix :math:`H(q)`, which is used in the rigid-body equation of motion (EoM) in joint
|
||||
space given by (see [1]):
|
||||
|
||||
@@ -3002,7 +3004,7 @@ class Bullet(Simulator):
|
||||
def calculate_inverse_kinematics(self, body_id, link_id, position, orientation=None, lower_limits=None,
|
||||
upper_limits=None, joint_ranges=None, rest_poses=None, joint_dampings=None,
|
||||
solver=None, q_curr=None, max_iters=None, threshold=None):
|
||||
"""
|
||||
r"""
|
||||
Compute the FULL Inverse kinematics; it will return a position for all the actuated joints.
|
||||
|
||||
"You can compute the joint angles that makes the end-effector reach a given target position in Cartesian world
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
#!/usr/bin/env python
|
||||
"""Provide few PyTorch utils methods.
|
||||
|
||||
For instance, it can evaluate the Hessian matrix of a scalar function.
|
||||
|
||||
References:
|
||||
[1] https://pytorch.org/
|
||||
[2] https://github.com/mariogeiger/hessian
|
||||
[3] https://github.com/Ageliss/For_shared_codes/blob/master/Second_order_gradients.py
|
||||
"""
|
||||
|
||||
import torch
|
||||
from torch.autograd import grad
|
||||
|
||||
# import hessian
|
||||
|
||||
|
||||
__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"
|
||||
|
||||
|
||||
def kronecker(A, B):
|
||||
"""
|
||||
Return the kronecker product between two matrices [1].
|
||||
|
||||
Args:
|
||||
A (torch.Tensor): 1st matrix.
|
||||
B (torch.Tensor): 2nd matrix.
|
||||
|
||||
Returns:
|
||||
torch.Tensor: kronecker product between two matrices
|
||||
|
||||
References:
|
||||
[1] Kronecker product (Wikipedia): https://discuss.pytorch.org/t/kronecker-product/3919/7
|
||||
[2] https://discuss.pytorch.org/t/kronecker-product/3919/7
|
||||
[3] Another implementation:
|
||||
https://github.com/cornellius-gp/gpytorch/blob/master/gpytorch/lazy/kronecker_product_lazy_tensor.py
|
||||
"""
|
||||
return torch.einsum("ab,cd->acbd", A, B).view(A.size(0)*B.size(0), A.size(1)*B.size(1))
|
||||
|
||||
|
||||
def hessian(f, model):
|
||||
r"""
|
||||
Get the Hessian matrix of the model.
|
||||
|
||||
Args:
|
||||
f:
|
||||
model:
|
||||
|
||||
Returns:
|
||||
torch.Tensor: Hessian matrix (which is a square symmetric and positive-definite real matrix)
|
||||
|
||||
References:
|
||||
[1] https://github.com/mariogeiger/hessian
|
||||
[2] https://github.com/Ageliss/For_shared_codes/blob/master/Second_order_gradients.py
|
||||
"""
|
||||
pass
|
||||
|
||||
|
||||
def hessian_vector(f, model):
|
||||
r"""
|
||||
Get the Hessian-vector product.
|
||||
|
||||
Args:
|
||||
f:
|
||||
model:
|
||||
|
||||
Returns:
|
||||
torch.Tensor: vector
|
||||
|
||||
References:
|
||||
[1] https://github.com/pytorch/pytorch/releases/tag/v0.2.0
|
||||
"""
|
||||
pass
|
||||
@@ -0,0 +1,66 @@
|
||||
#!/usr/bin/env python
|
||||
"""Convert units from one system to another one.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
__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"
|
||||
|
||||
|
||||
def inches_to_meters(inch):
|
||||
"""
|
||||
Convert the inches to meters.
|
||||
|
||||
Args:
|
||||
inch (float): inches.
|
||||
|
||||
Returns:
|
||||
float: meters
|
||||
"""
|
||||
return inch * 0.0254
|
||||
|
||||
|
||||
def meters_to_inches(meter):
|
||||
"""
|
||||
Convert the meters to inches.
|
||||
|
||||
Args:
|
||||
meter (float): meters.
|
||||
|
||||
Returns:
|
||||
float: inches
|
||||
"""
|
||||
return meter / 0.0254
|
||||
|
||||
|
||||
def rpm_to_rad_per_second(rpm):
|
||||
"""
|
||||
Convert the revolutions per minute to rad/sec.
|
||||
|
||||
Args:
|
||||
rpm (float): revolutions per minute.
|
||||
|
||||
Returns:
|
||||
float: rad/sec
|
||||
"""
|
||||
return rpm * 2 * np.pi/60
|
||||
|
||||
|
||||
def rad_per_second_to_rpm(omega):
|
||||
"""
|
||||
Convert rad/sec to revolutions/minute.
|
||||
|
||||
Args:
|
||||
omega (float): angular velocity (rad/sec)
|
||||
|
||||
Returns:
|
||||
float: revolutions/minute
|
||||
"""
|
||||
return omega * 60 / (2*np.pi)
|
||||
Reference in New Issue
Block a user