wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle, "",
Problem.SLIDING)
ps = ProblemSolver (robot)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot"])
# lock hands in closed position
lockedjoints = robot.leftHandClosed ()
for name, value in lockedjoints.iteritems ():
ps.lockJoint (name, value)
lockedjoints = robot.rightHandClosed ()
for name, value in lockedjoints.iteritems ():
ps.lockJoint (name, value)
q1 = [0.0, 0.0, 0.705, 0., 0., 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
res = ps.applyConstraints (q1)
if res [0]:
q1proj = res [1]
else:
raise RuntimeError ("Failed to apply constraint.")
# q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF#
q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0]
q3 = [0, -1.57, 1.57, 3.267256451, 0, 0]
q_init = q1;
# q_goal = q2
q_goal = q3
from hpp.gepetto import ViewerFactory, PathPlayerGui
vf = ViewerFactory (ps)
# vf.loadObstacleModel ("ur_description","obstacles","obstacles")
vf.loadObstacleModel ("ur_description","table","table")
# vf.loadObstacleModel ("ur_description","wall","wall")
vf(q1)
ps.lockJoint("elbow_joint", [q1[2]])
ps.selectPathValidation("Progressive", .05)
ps.setParameter("SplineGradientBased/alphaInit", CORBA.Any(CORBA.TC_double, 0.3))
ps.setParameter("SplineGradientBased/alwaysStopAtFirst", CORBA.Any(CORBA.TC_boolean, True))
ps.setParameter("SplineGradientBased/linearizeAtEachStep", CORBA.Any(CORBA.TC_boolean, False))
print "Optimizer parameters are:"
print "alphaInit:", ps.getParameter("SplineGradientBased/alphaInit").value()
print "alwaysStopAtFirst:", ps.getParameter("SplineGradientBased/alwaysStopAtFirst").value()
print "linearizeAtEachStep:", ps.getParameter("SplineGradientBased/linearizeAtEachStep").value()
print ""
import datetime as dt
totalSolveTime = dt.timedelta (0)
wcl = WsClient()
wcl.problem.addStaticStabilityConstraints("balance", q0, robot.leftAnkle,
robot.rightAnkle, "",
Problem.SLIDING)
ps = ProblemSolver(robot)
ps.setNumericalConstraints("balance", [
"balance/relative-com", "balance/relative-orientation",
"balance/relative-position", "balance/orientation-left-foot",
"balance/position-left-foot"
])
# lock hands in closed position
lockedjoints = robot.leftHandClosed()
for name, value in lockedjoints.iteritems():
ps.lockJoint(name, value)
lockedjoints = robot.rightHandClosed()
for name, value in lockedjoints.iteritems():
ps.lockJoint(name, value)
q1 = [
0.0, 0.0, 0.705, 0., 0., 0., 1.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0,
0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532,
0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532,
0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665,
-0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0
]
res = ps.applyConstraints(q1)
if res[0]:
'l_wrist_3_joint': -1.37711410223,
'r_shoulder_pan_joint': 1.57079632679,
'r_shoulder_lift_joint': -2.05619449019,
'r_elbow_joint': -0.2,
'r_wrist_1_joint': -2.61799387799,
'r_wrist_2_joint': 2.09439510239,
'r_wrist_3_joint': 0.785398163397,
}
q_goal = robot.halfSitting [::]
for j, v in dict_goal.iteritems ():
i = robot.rankInConfiguration [j]
q_goal [i] = v
for j, v in robot.openHand (None, 0.2, 'left').iteritems ():
ps.lockJoint (j, [v,])
for j, v in robot.openHand (None, 0.2, 'right').iteritems ():
ps.lockJoint (j, [v,])
ps.lockJoint ('base_joint_xy', [0,0])
ps.lockJoint ('base_joint_rz', [1,0])
res = ps.applyConstraints (q_init)
if res [0]:
q_init = res [1]
else:
raise RuntimeError ('Failed to apply constraints on init config.')
res = ps.applyConstraints (q_goal)
if res [0]: