def setup_ik_from_stance():
"""
Setup inverse kinematics from the simpler stance interface.
Notes
-----
This function is equivalent to :func:`setup_ik_from_tasks` below.
"""
robot.set_z(0.8) # hack to start with the robot above contacts
com_target = robot.get_com_point_mass()
lf_target = robot.left_foot.get_contact(pos=[0, 0.3, 0])
rf_target = robot.right_foot.get_contact(pos=[0, -0.3, 0])
stance = Stance(com=com_target, left_foot=lf_target, right_foot=rf_target)
stance.dof_tasks[robot.R_SHOULDER_R] = -0.5
stance.dof_tasks[robot.L_SHOULDER_R] = +0.5
stance.bind(robot)
def update_com_ds(self, sim):
try:
stance = Stance(
com=self.ds_com_target, left_foot=self.swing_start,
right_foot=self.support_contact)
self.com_ds = DoubleSupportController(
self.nb_ds_steps, 0.5, self.pendulum.omega2,
self.state_estimator, self.ds_com_target, stance)
sim.log_comp_time('dsqp_build', self.com_ds.lmpc.build_time)
if self.com_ds.lmpc.solve_time is not None:
sim.log_comp_time('dsqp_solve', self.com_ds.lmpc.solve_time)
if self.__draw_support_tube:
self.__ds_tube_handle = self.com_ds.tube.draw()
except RuntimeError as e: # this one is cdd...
print("%s error:" % type(self).__name__, e)
self.preview = None
self.tube = None
if __name__ == "__main__":
sim = pymanoid.Simulation(dt=0.03)
robot = pymanoid.robots.JVRC1()
set_torque_limits(robot)
robot.show_com()
sim.set_viewer()
sim.set_camera_transform(
numpy.array([[-0.75166831, -0.47792323, 0.45451529, -0.99502754],
[-0.65817994, 0.49930137, -0.563469, 1.14903212],
[0.04235481, -0.72269463, -0.68986849, 2.35493684],
[0., 0., 0., 1.]]))
robot.set_transparency(0.25)
stance = Stance.from_json('jvrc1_double_support.json')
stance.bind(robot)
robot.ik.solve()
polygon_height = stance.com.z # [m]
uncons_polygon = stance.compute_static_equilibrium_polygon(method="cdd")
h1 = draw_horizontal_polygon(uncons_polygon, polygon_height, color='g')
sim.schedule(robot.ik)
sim.start()
ada = ActuationDependentArea(robot, stance)
ada.sample_working_set(uncons_polygon, "sample", 20)
PLAY_WITH_LOCAL_POLYGONS = False
if PLAY_WITH_LOCAL_POLYGONS:
def on_tick(self, sim):
self.stance.com.set_x(self.com_above.x)
self.stance.com.set_y(self.com_above.y)
if __name__ == "__main__":
sim = pymanoid.Simulation(dt=0.03)
robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True)
sim.set_viewer()
sim.set_camera_top(x=0., y=0., z=3.)
robot.set_transparency(0.25)
com_above = pymanoid.Cube(0.02, [0.05, 0.04, z_polygon], color='b')
stance = Stance.from_json('../stances/double.json')
stance.com.hide()
stance.bind(robot)
robot.ik.solve()
method = "hull"
if "bretl" in sys.argv:
method = "bretl"
elif "cdd" in sys.argv:
method = "cdd"
com_sync = COMSync(stance, com_above)
polygon_drawer = SupportPolygonDrawer(stance, method)
wrench_drawer = StaticWrenchDrawer(stance)
sim.schedule(robot.ik)
print("- half-width = %s" % repr(contact.shape[1]))
print("- friction = %f" % contact.friction)
print("")
if __name__ == "__main__":
sim = pymanoid.Simulation(dt=0.03)
robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True)
sim.set_viewer()
sim.viewer.SetCamera([
[0.60587192, -0.36596244, 0.70639274, -2.4904027],
[-0.79126787, -0.36933163, 0.48732874, -1.6965636],
[0.08254916, -0.85420468, -0.51334199, 2.79584694],
[0., 0., 0., 1.]])
robot.set_transparency(0.25)
stance = Stance.from_json('stances/triple.json')
stance.bind(robot)
robot.ik.solve()
sim.schedule(robot.ik)
sim.start()
p = array([0., 0., 0.])
CWC_O = stance.compute_wrench_inequalities(p)
print_contact("Left foot", stance.left_foot)
print_contact("Right foot", stance.right_foot)
print("Contact Wrench Cone at %s:" % str(p))
print("- has %d lines" % CWC_O.shape[0])
if IPython.get_ipython() is None:
IPython.embed()
def generate_staircase(radius, angular_step, height, roughness, friction,
ds_duration, ss_duration, init_com_offset=None):
"""
Generate a new slanted staircase with tilted steps.
Parameters
----------
radius : scalar
Staircase radius in [m].
angular_step : scalar
Angular step between contacts in [rad].
height : scalar
Altitude variation in [m].
roughness : scalar
Amplitude of contact roll, pitch and yaw in [rad].
friction : scalar
Friction coefficient between a robot foot and a step.
ds_duration : scalar
Duration of double-support phases in [s].
ss_duration : scalar
Duration of single-support phases in [s].
init_com_offset : array, optional
Initial offset applied to first stance.
"""
stances = []
contact_shape = (0.12, 0.06)
first_left_foot = None
prev_right_foot = None
for theta in arange(0., 2 * pi, angular_step):
left_foot = Contact(
shape=contact_shape,
pos=[radius * cos(theta),
radius * sin(theta),
radius + .5 * height * sin(theta)],
rpy=(roughness * (random(3) - 0.5) + [0, 0, theta + .5 * pi]),
friction=friction)
if first_left_foot is None:
first_left_foot = left_foot
right_foot = Contact(
shape=contact_shape,
pos=[1.2 * radius * cos(theta + .5 * angular_step),
1.2 * radius * sin(theta + .5 * angular_step),
radius + .5 * height * sin(theta + .5 * angular_step)],
rpy=(roughness * (random(3) - 0.5) + [0, 0, theta + .5 * pi]),
friction=friction)
if prev_right_foot is not None:
com_target_pos = left_foot.p + [0., 0., JVRC1.leg_length]
com_target = PointMass(com_target_pos, robot.mass, visible=False)
dsl_stance = Stance(
com_target, left_foot=left_foot, right_foot=prev_right_foot)
dsl_stance.label = 'DS-L'
dsl_stance.duration = ds_duration
ssl_stance = Stance(com_target, left_foot=left_foot)
ssl_stance.label = 'SS-L'
ssl_stance.duration = ss_duration
dsl_stance.compute_static_equilibrium_polygon()
ssl_stance.compute_static_equilibrium_polygon()
stances.append(dsl_stance)
stances.append(ssl_stance)
com_target_pos = right_foot.p + [0., 0., JVRC1.leg_length]
if init_com_offset is not None:
com_target_pos += init_com_offset
init_com_offset = None
com_target = PointMass(com_target_pos, robot.mass, visible=False)
dsr_stance = Stance(
com_target, left_foot=left_foot, right_foot=right_foot)
dsr_stance.label = 'DS-R'
dsr_stance.duration = ds_duration
ssr_stance = Stance(com_target, right_foot=right_foot)
ssr_stance.label = 'SS-R'
ssr_stance.duration = ss_duration
dsr_stance.compute_static_equilibrium_polygon()
ssr_stance.compute_static_equilibrium_polygon()
stances.append(dsr_stance)
stances.append(ssr_stance)
prev_right_foot = right_foot
com_target_pos = first_left_foot.p + [0., 0., JVRC1.leg_length]
com_target = PointMass(com_target_pos, robot.mass, visible=False)
dsl_stance = Stance(
com_target, left_foot=first_left_foot, right_foot=prev_right_foot)
dsl_stance.label = 'DS-L'
dsl_stance.duration = ds_duration
ssl_stance = Stance(com_target, left_foot=first_left_foot)
ssl_stance.label = 'SS-L'
ssl_stance.duration = ss_duration
dsl_stance.compute_static_equilibrium_polygon()
ssl_stance.compute_static_equilibrium_polygon()
stances.append(dsl_stance)
stances.append(ssl_stance)
return stances
[[-0.43496840, 0.32393275, -0.84016074, 2.07345104],
[+0.88804317, 0.30865173, -0.34075422, 0.77617097],
[+0.14893562, -0.89431632, -0.42192002, 1.63681912],
[+0., 0., 0., 1.]])
contact = pymanoid.Contact(
shape=(0.12, 0.06),
pos=[0.15, -0.15, 0.],
# rpy=[-0.19798375, 0.13503151, 0],
rpy=[-0.35, -0.35, 0.05],
friction=0.7)
mass = 38. # [kg]
z_target = 0.8 # [m]
init_pos = array([0., 0., z_target])
init_vel = 4. * (contact.p - init_pos) * array([1., 1., 0.])
pendulum = InvertedPendulum(mass, init_pos, init_vel, contact, z_target)
stance = Stance(com=pendulum.com, right_foot=contact)
stance.bind(robot)
robot.ik.solve()
g = -sim.gravity[2]
lambda_min = 0.1 * g
lambda_max = 2.0 * g
stabilizer_2d = Stabilizer2D(pendulum, lambda_min, lambda_max, nb_steps=10)
stabilizer_3d = Stabilizer3D(pendulum,
lambda_min,
lambda_max,
nb_steps=10,
cop_gain=2.)
sim.schedule(stabilizer_2d)
sim.schedule(stabilizer_3d)
if __name__ == "__main__":
sim = pymanoid.Simulation(dt=0.03)
robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True)
sim.set_viewer()
sim.viewer.SetCamera([[0.60587192, -0.36596244, 0.70639274, -2.4904027],
[-0.79126787, -0.36933163, 0.48732874, -1.6965636],
[0.08254916, -0.85420468, -0.51334199, 2.79584694],
[0., 0., 0., 1.]])
robot.set_transparency(0.25)
stance = Stance(com=PointMass(pos=[0., 0., 0.9], mass=robot.mass),
left_foot=Contact(shape=robot.sole_shape,
pos=[0.20, 0.15, 0.1],
rpy=[0.4, 0, 0],
friction=0.5),
right_foot=Contact(shape=robot.sole_shape,
pos=[-0.2, -0.195, 0.],
rpy=[-0.4, 0, 0],
friction=0.5))
stance.bind(robot)
robot.ik.solve()
sim.schedule(robot.ik)
sim.start()
p = array([0., 0., 0.])
CWC_O = stance.compute_wrench_inequalities(p)
print_contact("Left foot", stance.left_foot)
print_contact("Right foot", stance.right_foot)
print "Contact Wrench Cone at %s:" % str(p)
print "- has %d lines" % CWC_O.shape[0]
[0.08254916, -0.85420468, -0.51334199, 2.79584694],
[0., 0., 0., 1.]])
robot.set_transparency(0.25)
com_target = PointMass(pos=[0., 0., com_height],
mass=robot.mass,
color='b',
visible=False)
com_above = pymanoid.Cube(0.02, [0.05, 0.04, z_polygon], color='b')
stance = Stance(com=com_target,
left_foot=Contact(shape=robot.sole_shape,
pos=[0.20, 0.15, 0.1],
rpy=[0.4, 0, 0],
static_friction=0.5,
visible=True),
right_foot=Contact(shape=robot.sole_shape,
pos=[-0.2, -0.195, 0.],
rpy=[-0.4, 0, 0],
static_friction=0.5,
visible=True))
robot.init_ik(active_dofs=robot.whole_body)
robot.set_dof_values([
3.53863816e-02, 2.57657518e-02, 7.75586039e-02, 6.35909636e-01,
7.38580762e-02, -5.34226902e-01, -7.91656626e-01, 1.64846093e-01,
-2.13252247e-01, 1.12500819e+00, -1.91496369e-01, -2.06646315e-01,
1.39579597e-01, -1.33333598e-01, -8.72664626e-01, 0.00000000e+00,
-9.81307787e-15, 0.00000000e+00, -8.66484961e-02, -1.78097540e-01,
-1.68940240e-03, -5.31698601e-01, -1.00166891e-04, -6.74394930e-04,
-1.01552628e-04, -5.71121132e-15, -4.18037117e-15, 0.00000000e+00,
robot.set_z(0.8) # hack to start with the robot above contacts
lf_target = robot.left_foot.get_contact(pos=[0, 0.1, 0])
rf_target = robot.right_foot.get_contact(pos=[0, -0.1, 0])
lh_target = robot.left_hand.get_contact(pos=[0, 0.5, 1.0])
# lh_target.set_transform(np.array([[0,1,0,0],[0,0,1,0.6],[1,0,0,1.5],[0,0,0,1]]))
lh_target.set_transform(np.array([[0,0,1,0],[1,0,0,0.6],[0,1,0,1.5],[0,0,0,1]]))
rh_target = robot.right_hand.get_contact(pos=[0, -0.5, 1.0])
# rh_target.set_transform(np.array([[0,-1,0,0],[0,0,-1,-0.6],[1,0,0,1.5],[0,0,0,1]]))
rh_target.set_transform(np.array([[0,0,1,0],[-1,0,0,-0.6],[0,-1,0,1.5],[0,0,0,1]]))
com_target = robot.get_com_point_mass()
com_target.set_pos(np.array([0,0,1.2]))
start = time.time()
stance = Stance(com=com_target, left_foot=lf_target, right_foot=rf_target, left_hand=lh_target, right_hand=rh_target)
stance.dof_tasks[robot.R_SHOULDER_R] = -0.5
stance.dof_tasks[robot.L_SHOULDER_R] = +0.5
stance.bind(robot)
robot.ik.solve(max_it=100, impr_stop=1e-4)
end = time.time()
print(end-start)
sim.schedule(robot.ik)
sim.start()
if IPython.get_ipython() is None: # give the user a prompt
IPython.embed()
robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True)
sim.set_viewer()
sim.viewer.SetCamera([[0.2753152, 0.29704774, -0.91431077, 2.89042521],
[0.96034717, -0.04146411, 0.27570643, -0.59789598],
[0.04398688, -0.95396193, -0.29668466, 1.65848958],
[0., 0., 0., 1.]])
robot.set_transparency(0.25)
stance = Stance(com=PointMass(pos=[0.1, 0.2, 0.9], mass=robot.mass),
left_foot=Contact(shape=robot.sole_shape,
friction=0.7,
pos=[0.3, 0.39, 0.10],
link=robot.left_foot,
rpy=[0.15, 0., -0.02]),
right_foot=Contact(shape=robot.sole_shape,
friction=0.7,
pos=[0., -0.15, 0.02],
link=robot.right_foot,
rpy=[-0.3, 0., 0.]),
right_hand=Contact(shape=(0.04, 0.04),
friction=0.5,
pos=[0.1, -0.5, 1.2],
rpy=[-1.57, 0., 0.]))
stance.bind(robot)
robot.ik.solve()
soft_contact = SoftContact(stance.right_hand)
wrench_drawer = RobotWrenchDrawer(robot)
sim.schedule(robot.ik)
sim.schedule(soft_contact)
-8.45611535e-01, 1.39994759e-02, 1.17756934e-16,
3.14018492e-16, -3.17943723e-15, -6.28036983e-16,
-3.17943723e-15, -6.28036983e-16, -6.88979202e-02,
-4.90099381e-02, 8.17415141e-01, -8.71841480e-02,
-1.36966665e-01, -4.26226421e-02])
com_target = PointMass(pos=[0., 0., com_height], mass=robot.mass, color='b')
com_target.hide()
com_above = pymanoid.Cube(0.02, [0.05, 0.04, z_polygon], color='b')
stance = Stance(
com=com_target,
left_foot=Contact(
shape=robot.sole_shape,
pos=[0.20, 0.15, 0.1],
rpy=[0.4, 0, 0],
friction=0.5),
right_foot=Contact(
shape=robot.sole_shape,
pos=[-0.2, -0.195, 0.],
rpy=[-0.4, 0, 0],
friction=0.5))
stance.bind(robot)
robot.ik.solve()
com_sync = COMSync()
cone_drawer = COMAccelConeDrawer(stance, scale=0.05)
sep_drawer = SEPDrawer(stance, z_polygon)
wrench_drawer = StaticWrenchDrawer(com_target, stance)
zmp_area_drawer = ZMPSupportAreaDrawer(stance, z_polygon)
sim.schedule(robot.ik)
robot.L_UINDEX, robot.L_ULITTLE
])
robot.set_dof_values(-q_fingers, [
robot.R_LTHUMB, robot.R_LINDEX, robot.R_LLITTLE, robot.R_UTHUMB,
robot.R_UINDEX, robot.R_ULITTLE
])
sim.set_viewer()
sim.set_camera_transform(
array([[-0.75318301, -0.33670976, 0.56510344, -1.27825475],
[-0.65389426, 0.28962469, -0.69895625, 1.3535881],
[0.07167748, -0.89595987, -0.43831297, 1.87996328],
[0., 0., 0., 1.]]))
robot.set_transparency(0.25)
stance = Stance.from_json('jvrc1_ladder_climbing.json')
stance.bind(robot)
robot.ik.maximize_margins = True
robot.ik.verbosity = 0
robot.ik.solve(impr_stop=1e-3)
from pymanoid.tasks import AxisAngleContactTask
if False:
del robot.ik.tasks['left_hand_palm']
del robot.ik.tasks['right_hand_palm']
lh_task = AxisAngleContactTask(robot,
robot.left_hand,
stance.left_hand,
weight=1,
gain=0.8)
