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