robot = Robot('hrp2_14')
robot.setJointBounds("root_joint",
[-3, 3, -3, 3, 0, 1, -1, 1, -1, 1, -1, 1, -1, 1])
cl = robot.client
q0 = robot.getInitialConfig()
# Add constraints
ps = ProblemSolver(robot)
ps.addPartialCom('hrp2_14', ['root_joint'])
robot.createSlidingStabilityConstraint("balance/", 'hrp2_14', robot.leftAnkle,
robot.rightAnkle, q0)
ps.setNumericalConstraints("balance", [
"balance/relative-com",
"balance/relative-pose",
"balance/pose-left-foot",
])
# lock hands in closed position
lockedjointDict = robot.leftHandClosed()
lockedJoints = list()
for name, value in lockedjointDict.iteritems():
ljName = "locked_" + name
ps.createLockedJoint(ljName, name, value)
lockedJoints.append(ljName)
lockedjointDict = robot.rightHandClosed()
for name, value in lockedjointDict.iteritems():
ljName = "locked_" + name
ps.createLockedJoint(ljName, name, value)
robot.setTranslationBounds(-3, 3, -3, 3, 0, 1)
cl = robot.client
r = ScenePublisher(robot)
q0 = robot.getInitialConfig()
r(q0)
# Add constraints
wcl = WsClient()
wcl.problem.addStaticStabilityConstraints("balance", q0, robot.leftAnkle,
robot.rightAnkle)
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
lockedDofs = robot.leftHandClosed()
for name, value in lockedDofs.iteritems():
ps.lockDof(name, value, 0, 0)
lockedDofs = robot.rightHandClosed()
for name, value in lockedDofs.iteritems():
ps.lockDof(name, value, 0, 0)
q1 = [
0.0, 0.0, 0.705, 1.0, 0., 0., 0.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,
q_goal [0:2] = [1.05,0]
# 3}}}
# Generate constraints {{{3
constraintName = "balance"
wcl.problem.addStaticStabilityConstraints (constraintName, q_init, robot.leftAnkle,
robot.rightAnkle, "",
Problem.SLIDING)
balanceConstraints = [constraintName + "/relative-com",
constraintName + "/relative-orientation",
constraintName + "/relative-position",
constraintName + "/orientation-left-foot",
constraintName + "/position-left-foot"]
# 3}}}
ps.setNumericalConstraints ("balance", balanceConstraints);
ps.setInitialConfig (q_init)
r = Viewer (ps)
r(q_init)
### Walk forward
if walk_forward:
ps.addGoalConfig (q_goal)
ps.solve ()
print "Solved forward"
### Walk sideway
q_goal_side = q_init[:]
q_goal_side [0:2] = [0, 0.95]
if walk_sideway:
robot = Robot ('hrp2_14')
robot.setJointBounds ("base_joint_xyz", (-3, 3, -3, 3, 0, 1))
cl = robot.client
q0 = robot.getInitialConfig ()
# Add constraints
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]
q2 [0:2] = [-1.5, -4] # x, y
#q2 [2:4] = [-0.707, 0.707] # theta
q2 [2:4] = [-1, 0] # theta
q2 [robot.rankInConfiguration ['torso_lift_joint']] = 0.02
r (q2)
## CONSTRAINTS ##
# Relative position constraint between PR2's right hand and the set's right handle.
ps.createPositionConstraint ("posConstraint1", "r_gripper_r_finger_joint", "j_marker_set", [0,0,0], [0.8455, -0.089, 0.01], [1,1,1])
#ps.createPositionConstraint ("posConstraint2", "r_gripper_l_finger_joint", "j_marker_set", [0,0,0], [0.841, 0.001, -0.0248], [1,1,1]) #WRONG!
# Glogal orientation constraint of the set that has to stay horizontal.
ps.createOrientationConstraint ("orConstraint", "j_marker_set", "", [0.707106781,0,0,-0.707106781], [1,1,0])
ps.setNumericalConstraints ("constraints", ["posConstraint1","orConstraint"])
res = ps.applyConstraints (q1)
if res [0]:
q1proj = res [1]
else:
raise RuntimeError ("Failed to apply constraint.")
if not(robot.isConfigValid(q1proj)):
raise RuntimeError ("Projected config non valid.")
res = ps.applyConstraints (q2)
if res [0]:
q2proj = res [1]
else:
# q = [x, y, z, theta] # (z not considered since planar)
q1 = [-4, 4, 1, 0]; q2 = [4, -4, 1, 0] # obstS 1
#q1 = [2.4, -4.6, 1.0, 0.0]; q2 = [-0.4, 4.6, 1.0, 0.0] # obstS 2
ps.setInitialConfig (q1)
ps.addGoalConfig (q2)
# Load box obstacle in HPP for collision avoidance #
#cl.obstacle.loadObstacleModel('potential_description','cylinder_obstacle','')
cl.obstacle.loadObstacleModel('potential_description','obstacles_concaves','')
r.loadObstacleModel ("potential_description","obstacles_concaves","obstacles_concaves") # in viewer !
ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [0.7071067812
,0,0,0.7071067812], [0,0,1]) # OK
T = [[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,1]]
ps.setNumericalConstraints ("constraints", ["orConstraint"])
ps.solve ()
begin=time.time()
cl.problem.optimizePath(0)
end=time.time()
print "Solving time: "+str(end-begin)
len(cl.problem.nodes ())
cl.problem.pathLength(0)
cl.problem.pathLength(1)
## Debug Optimization Tools ##############
import matplotlib.pyplot as plt
num_log = 32234
import time
robot = Robot('squel_robot')
robot.setJointBounds('base_joint_xyz', [-3, 3, -3, 3, 0, 1])
ps = ProblemSolver(robot)
cl = robot.client
Viewer.withFloor = True
r = Viewer(ps)
#r.loadObstacleModel ("room_description","room","room")
#r.loadObstacleModel ("room_description","walls","walls")
#pp = PathPlayer (cl, r)
#r.loadObstacleModel ("room_description","squel","squel")
# Add constraints
"""
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot"])
"""
q1 = [
0.0, 0.0, 1.2, 0, 0, 1.57, 0.0, 0.0, 0.0, 0.0, 0, 0, 0, 0, 0.0, 0.0, 0, 0,
0, -0.174532, 0, 0, 0, 0, 0.0, 0, 0.0, 0.0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, 0,
0, 0, 0.0, 0.0, 0.0, 0, 0, 0, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
wcl.problem.addStaticStabilityConstraints ("balance", q, robot.leftAnkle,
robot.rightAnkle)
# Define quatenion for righthand orientation constraints
#quat = Quaternion()
#quat.fromRPY(-1.55, -1.55, 0.0) # Right hand horizontal, grasping to -z
#quat.fromRPY(-1.55, 0.0, 0.0) # Right hand vertical, pointing down
#quat.fromRPY(0.0, 0.0, -1.55) # Right hand vertical, pointing forward
#quat.normalize()
ps.resetConstraints()
# Add constraints to problem solver
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot",])
ps.createPositionConstraint("RightHandPos", "RWristPitch", "", [0.0, 0.0, 0.0], [-2.6, -4, 0.75], True, True, True)
#ps.createOrientationConstraint("RightHandOr", "", "RWristPitch", [quat.array[0], quat.array[1], quat.array[2], quat.array[3]], True, True, True)
#ps.setNumericalConstraints ("eef", ["RightHandPos", "RightHandOr"])
ps.setNumericalConstraints ("eef", ["RightHandPos"])
ps.createPositionConstraint("LeftAnklePos", robot.leftAnkle, "", [0.0, 0.0, 0.0], [robot.getJointPosition(robot.leftAnkle)[0], robot.getJointPosition(robot.leftAnkle)[1], robot.getJointPosition(robot.leftAnkle)[2]], True, True, False)
ps.createOrientationConstraint("LeftAnkleOr", "", robot.leftAnkle, [robot.getJointPosition(robot.leftAnkle)[3], robot.getJointPosition(robot.leftAnkle)[4], robot.getJointPosition(robot.leftAnkle)[5], robot.getJointPosition(robot.leftAnkle)[6]], False, False, True)
ps.setNumericalConstraints ("lock", ["LeftAnklePos", "LeftAnkleOr"])
ps.lockOneDofJoint("LFinger11", robot.getJointDofValue("LFinger11"))
ps.lockOneDofJoint("LFinger12", robot.getJointDofValue("LFinger12"))
ps.lockOneDofJoint("LFinger13", robot.getJointDofValue("LFinger13"))
ps.lockOneDofJoint("LFinger21", robot.getJointDofValue("LFinger21"))
import time
robot = Robot ('squel_robot')
robot.setJointBounds ('base_joint_xyz', [-3, 3, -3, 3, 0, 1])
ps = ProblemSolver (robot)
cl = robot.client
Viewer.withFloor = True
r = Viewer (ps)
#r.loadObstacleModel ("room_description","room","room")
#r.loadObstacleModel ("room_description","walls","walls")
#pp = PathPlayer (cl, r)
#r.loadObstacleModel ("room_description","squel","squel")
# Add constraints
"""
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot"])
"""
q1 = [0.0, 0.0, 1.2, 0, 0, 1.57, 0.0, 0.0, 0.0, 0.0, 0, 0, 0, 0, 0.0, 0.0, 0, 0, 0, -0.174532, 0, 0, 0, 0, 0.0, 0, 0.0, 0.0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, 0, 0, 0, 0.0, 0.0, 0.0, 0, 0, 0, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
r(q1)
"""
