#!/usr/bin/python #coding=utf-8 #author=Joseph Salini #date=15 june 2011 from common import create_icub_and_init, get_usual_observers from arboris.robots import icub from arboris.core import NamedObjectsList from numpy import pi, array, zeros ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_icub_and_init(chair=True, gravity=True) joints = w.getjoints() frames = w.getframes() bodies = w.getbodies() icub_joints = [joints[n] for n in icub.get_joints_data()] icub_bodies = [bodies[n] for n in icub.get_bodies_data()] standing_pose = [j.gpos[0] for j in icub_joints] joints['l_knee'].gpos[:] = -pi/2 joints['r_knee'].gpos[:] = -pi/2 joints['l_ankle_pitch'].gpos[:] = -pi/10 joints['r_ankle_pitch'].gpos[:] = -pi/10 joints['l_hip_pitch'].gpos[:] = pi/2 - pi/10
posdes = self.amp * sin(t * 2. * pi / self.period)
# posdes = self.amp*sin(t*2.*pi/self.period + pi/2) - self.amp
pos = self.parentjoint.gpos
vel = self.parentjoint.gvel
force = self.kp * (posdes - pos) - self.kd * vel
gforce[self.parentjoint.dof] = force
gforce[self.parentjoint.dof] += self.mass * 9.81
return gforce, impedance
#################################
# #
# CREATE WORLD & INITIALIZATION #
# #
#################################
w = create_icub_and_init(gravity=True)
add_table(w,
hl=(.05, .2, .02),
init_pos=transl(-.3, 0, .6),
mass=10.,
name='wall')
joints = w.getjoints()
frames = w.getframes()
bodies = w.getbodies()
shapes = w.getshapes()
consts = w.getconstraints()
##POS of the arms, before or after the rec of standing_pos?
joints['l_shoulder_pitch'].gpos[:] = -0.65695599
#author=Joseph Salini
#date=16 june 2011
from common import create_icub_and_init, get_usual_observers
from arboris.robots import icub
from arboris.homogeneousmatrix import rotx
from numpy import pi, eye, zeros
#################################
# #
# CREATE WORLD & INITIALIZATION #
# #
#################################
w = create_icub_and_init(gravity=True)
joints = w.getjoints()
frames = w.getframes()
bodies = w.getbodies()
frames = w.getframes()
const = w.getconstraints()
icub_joints = [joints[n] for n in icub.get_joints_data()]
icub_bodies = [bodies[n] for n in icub.get_bodies_data()]
standing_pose = [j.gpos[0] for j in icub_joints]
back_joints = [joints[n] for n in ['torso_pitch', 'torso_roll', 'torso_yaw', 'head_pitch', 'head_roll', 'head_yaw']]
arms_joints = [joints[n] for n in ['l_shoulder_pitch', 'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow_pitch', 'l_elbow_yaw', 'l_wrist_roll', 'l_wrist_pitch']+
['r_shoulder_pitch', 'r_shoulder_roll', 'r_shoulder_yaw', 'r_elbow_pitch', 'r_elbow_yaw', 'r_wrist_roll', 'r_wrist_pitch']]
arms_goal = [j.gpos[0] for j in arms_joints]
l_const = [c for c in const if c.name[0:2] == 'lf']
