def createStaticStabilityConstraint(self, prefix, comName, leftAnkle,
rightAnkle, q0):
robot = self.hppcorba.robot
problem = self.hppcorba.problem
_tfs = robot.getJointsPosition(q0, (leftAnkle, rightAnkle))
Ml = Transform(_tfs[0])
Mr = Transform(_tfs[1])
self.setCurrentConfig(q0)
x = self._getCOM(comName)
result = []
# COM wrt left ankle frame
xloc = Ml.inverse().transform(x)
result.append(prefix + "relative-com")
problem.createRelativeComConstraint(result[-1], comName, leftAnkle,
xloc.tolist(), (True, ) * 3)
# Pose of the left foot
result.append(prefix + "pose-left-foot")
problem.createTransformationConstraint2(
result[-1], "", leftAnkle, Ml.toTuple(), (0, 0, 0, 0, 0, 0, 1),
(True, True, True, True, True, True))
# Pose of the right foot
result.append(prefix + "pose-right-foot")
problem.createTransformationConstraint2(
result[-1], "", rightAnkle, Mr.toTuple(), (0, 0, 0, 0, 0, 0, 1),
(True, True, True, True, True, True))
return result
def createAlignedCOMStabilityConstraint (self, prefix, comName, leftAnkle, rightAnkle, q0, sliding):
robot = self.hppcorba.robot
problem = self.hppcorba.problem
_tfs = robot.getJointsPosition (q0, (leftAnkle, rightAnkle))
Ml = Transform(_tfs[0])
Mr = Transform(_tfs[1])
robot.setCurrentConfig (q0)
x = self._getCOM (robot, comName)
result = []
# COM between feet
result.append (prefix + "com-between-feet")
problem.createComBetweenFeet (result[-1], comName, leftAnkle, rightAnkle,
(0,0,0), (0,0,0), "", x.tolist(), (True,)*4)
if sliding:
mask = ( False, False, True, True, True, False )
else:
mask = ( True, ) * 6
# Pose of the right foot
result.append (prefix + "pose-right-foot")
problem.createTransformationConstraint2 (result[-1],
"", rightAnkle, Mr.toTuple(), (0,0,0,0,0,0,1), mask)
# Pose of the left foot
result.append (prefix + "pose-left-foot")
problem.createTransformationConstraint2 (result[-1],
"", leftAnkle, Ml.toTuple(), (0,0,0,0,0,0,1), mask)
return result;
def createStaticStabilityConstraint (self, prefix, comName, leftAnkle, rightAnkle, q0):
robot = self.hppcorba.robot
problem = self.hppcorba.problem
_tfs = robot.getJointsPosition (q0, (leftAnkle, rightAnkle))
Ml = Transform(_tfs[0])
Mr = Transform(_tfs[1])
self.setCurrentConfig (q0)
x = self._getCOM (comName)
result = []
# COM wrt left ankle frame
xloc = Ml.inverse().transform(x)
result.append (prefix + "relative-com")
problem.createRelativeComConstraint (result[-1], comName, leftAnkle, xloc.tolist(), (True,)*3)
# Pose of the left foot
result.append (prefix + "pose-left-foot")
problem.createTransformationConstraint2 (result[-1],
"", leftAnkle, Ml.toTuple(), (0,0,0,0,0,0,1), (True,True,True,True,True,True))
# Pose of the right foot
result.append (prefix + "pose-right-foot")
problem.createTransformationConstraint2 (result[-1],
"", rightAnkle, Mr.toTuple(), (0,0,0,0,0,0,1), (True,True,True,True,True,True))
return result
def addAlignmentConstrainttoEdge(ps, handle, graph):
#recover id of handle
handleId = all_handles.index(handle)
J1, gripperPose = ps.robot.getGripperPositionInJoint(tool_gripper)
J2, handlePose = ps.robot.getHandlePositionInJoint(handle)
T1 = Transform(gripperPose)
T2 = Transform(handlePose)
constraintName = handle + '/alignment'
ps.client.basic.problem.createTransformationConstraint2\
(constraintName, J1, J2, T1.toTuple(), T2.toTuple(),
[False, True, True, False, False, False])
# Set constraint
edgeName = tool_gripper + ' > ' + handle + ' | 0-0_12'
graph.addConstraints(edge = edgeName, constraints = \
Constraints(numConstraints=[constraintName]))
edgeName = tool_gripper + ' < ' + handle + ' | 0-0:1-{}_21'.format(handleId)
graph.addConstraints(edge = edgeName, constraints = \
Constraints(numConstraints=[constraintName]))
def createAlignedCOMStabilityConstraint(self, prefix, comName, leftAnkle,
rightAnkle, q0, sliding):
robot = self.hppcorba.robot
problem = self.hppcorba.problem
_tfs = robot.getJointsPosition(q0, (leftAnkle, rightAnkle))
Ml = Transform(_tfs[0])
Mr = Transform(_tfs[1])
robot.setCurrentConfig(q0)
x = self._getCOM(robot, comName)
result = []
# COM between feet
result.append(prefix + "com-between-feet")
problem.createComBetweenFeet(
result[-1],
comName,
leftAnkle,
rightAnkle,
(0, 0, 0),
(0, 0, 0),
"",
x.tolist(),
(True, ) * 4,
)
if sliding:
mask = (False, False, True, True, True, False)
else:
mask = (True, ) * 6
# Pose of the right foot
result.append(prefix + "pose-right-foot")
problem.createTransformationConstraint2(result[-1], "", rightAnkle,
Mr.toTuple(),
(0, 0, 0, 0, 0, 0, 1), mask)
# Pose of the left foot
result.append(prefix + "pose-left-foot")
problem.createTransformationConstraint2(result[-1], "", leftAnkle,
Ml.toTuple(),
(0, 0, 0, 0, 0, 0, 1), mask)
return result
