wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

update robot: add dynamics (link/joint acc.), augmented joint states, spatial link states + compute Jdot

Browse Source
This commit is contained in:
Brian Delhaisse
2019-07-14 17:10:21 +02:00
parent 60ce22f21e
commit 3250584a01
5 changed files with 923 additions and 244 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/robots/base.py
+6
View File
@@ -323,6 +323,12 @@ class Body(object):
# Methods #
###########
def step(self):
"""
Perform a step. This can be implemented in the child classes.
"""
pass
def set_color(self, color, link_id=-1):
"""Set the given RGBA color to the specified link. This is only valid in the simulator.
pyrobolearn/robots/robot.py
+837 -204
View File
File diff suppressed because it is too large Load Diff
pyrobolearn/simulators/bullet.py
+35 -34
View File
@@ -457,6 +457,7 @@ class Bullet(Simulator):
Args:
enable (bool): If True, it will enable the real-time simulation. If False, it will disable it.
"""
super(Bullet, self).set_real_time(enable=enable)
self.sim.setRealTimeSimulation(enableRealTimeSimulation=int(enable))
def pause(self):
@@ -2274,40 +2275,40 @@ class Bullet(Simulator):
# set the joint torques
self.set_joint_torques(body_id, joint_ids, torques)
def get_joint_accelerations(self, body_id, joint_ids, q=None, dq=None):
"""
Get the acceleration at the given joint(s). This is carried out by first getting the joint torques, then
performing forward dynamics to get the joint accelerations from the joint torques.
Args:
body_id (int): unique body id.
joint_ids (int, list of int): joint id, or list of joint ids.
q (list of int, None): all the joint positions. If None, it will compute it.
dq (list of int, None): all the joint velocities. If None, it will compute it.
Returns:
if 1 joint:
float: joint acceleration [rad/s^2]
if multiple joints:
np.array[N]: joint accelerations [rad/s^2]
"""
# get the torques
torques = self.get_joint_torques(body_id, joint_ids)
# get position and velocities
if q is None or dq is None:
joints = self.get_actuated_joint_ids(body_id)
if q is None:
q = self.get_joint_positions(body_id, joints)
if dq is None:
dq = self.get_joint_velocities(body_id, joints)
# compute the accelerations
accelerations = self.calculate_forward_dynamics(body_id, q, dq, torques=torques)
# return the specified accelerations
q_idx = self.get_q_indices(body_id, joint_ids)
return accelerations[q_idx]
# def get_joint_accelerations(self, body_id, joint_ids): # , q=None, dq=None):
# """
# Get the acceleration at the given joint(s). This is carried out by first getting the joint torques, then
# performing forward dynamics to get the joint accelerations from the joint torques.
#
# Args:
# body_id (int): unique body id.
# joint_ids (int, list of int): joint id, or list of joint ids.
# q (list of int, None): all the joint positions. If None, it will compute it.
# dq (list of int, None): all the joint velocities. If None, it will compute it.
#
# Returns:
# if 1 joint:
# float: joint acceleration [rad/s^2]
# if multiple joints:
# np.array[N]: joint accelerations [rad/s^2]
# """
# # get the torques
# torques = self.get_joint_torques(body_id, joint_ids)
#
# # get position and velocities
# if q is None or dq is None:
# joints = self.get_actuated_joint_ids(body_id)
# if q is None:
# q = self.get_joint_positions(body_id, joints)
# if dq is None:
# dq = self.get_joint_velocities(body_id, joints)
#
# # compute the accelerations
# accelerations = self.calculate_forward_dynamics(body_id, q, dq, torques=torques)
#
# # return the specified accelerations
# q_idx = self.get_q_indices(body_id, joint_ids)
# return accelerations[q_idx]
def set_joint_torques(self, body_id, joint_ids, torques):
"""
pyrobolearn/simulators/simulator.py
+37 -6
View File
@@ -387,7 +387,11 @@ class Simulator(object):
Args:
enable (bool): If True, it will enable the real-time simulation. If False, it will disable it.
"""
pass
self.real_time = True
def use_real_time(self):
"""Return True if the simulator is in real-time mode."""
return self.real_time
def pause(self):
"""Pause the simulator if in real-time."""
@@ -929,6 +933,19 @@ class Simulator(object):
"""
pass
def get_base_acceleration(self, body_id):
"""
Get the base acceleration. This is only valid if the simulator `supports_acceleration`.
Args:
body_id (int): unique object id.
Returns:
np.array[3]: linear acceleration [m/s^2]
np.array[3]: angular acceleration [rad/s^2]
"""
pass
def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=(0., 0., 0.), frame=1):
"""
Apply the specified external force on the specified position on the body / link.
@@ -1282,6 +1299,23 @@ class Simulator(object):
def get_link_velocities(self, body_id, link_ids):
pass
def get_link_world_accelerations(self, body_id, link_ids):
"""
Return the linear and angular accelerations (expressed in the Cartesian world space coordinates) for the given
link(s). This is only valid if the simulator `supports_acceleration`.
Args:
body_id (int): unique body id.
link_ids (list of int): list of link indices.
Returns:
if 1 link:
np.array[6]: linear and angular acceleration of the link in the Cartesian world space
if multiple links:
np.array[N,6]: linear and angular acceleration of each link
"""
pass
def get_q_indices(self, body_id, joint_ids):
"""
Get the corresponding q index of the given joint(s).
@@ -1528,16 +1562,13 @@ class Simulator(object):
"""
pass
def get_joint_accelerations(self, body_id, joint_ids, q=None, dq=None):
def get_joint_accelerations(self, body_id, joint_ids): # , q=None, dq=None):
"""
Get the acceleration at the given joint(s). This is carried out by first getting the joint torques, then
performing forward dynamics to get the joint accelerations from the joint torques.
Get the acceleration of the specified joint(s). This is only valid if the simulator `supports_acceleration`.
Args:
body_id (int): unique body id.
joint_ids (int, list of int): joint id, or list of joint ids.
q (list of int, None): all the joint positions. If None, it will compute it.
dq (list of int, None): all the joint velocities. If None, it will compute it.
Returns:
if 1 joint:
pyrobolearn/worlds/world.py
+8
View File
@@ -388,7 +388,15 @@ class World(object):
# interface.step()
# for bridge in self.bridges:
# bridge.step()
# call the step method for each body
for body in self.bodies:
body.step()
# call simulation step
self.sim.step()
# sleep
if sleep_dt is not None:
time.sleep(sleep_dt)