コード例 #1
0
    def createSlidingStabilityConstraint (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)

        # Relative pose of the feet
        result.append (prefix + "relative-pose")
        problem.createTransformationConstraint2 (result[-1],
            leftAnkle, rightAnkle, (0,0,0,0,0,0,1), (Mr.inverse()*Ml).toTuple(), (True,)*6)

        # 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), (False,False,True,True,True,False))

        # Complement left foot
        result.append (prefix + "pose-left-foot-complement")
        problem.createTransformationConstraint2 (result[-1],
            "", leftAnkle, Ml.toTuple(), (0,0,0,0,0,0,1), (True,True,False,False,False,True))
        problem.setConstantRightHandSide (result[-1], False)

        return result
コード例 #2
0
    def createSlidingStabilityConstraint(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)

        # Relative pose of the feet
        result.append(prefix + "relative-pose")
        problem.createTransformationConstraint2(
            result[-1],
            leftAnkle,
            rightAnkle,
            (0, 0, 0, 0, 0, 0, 1),
            (Mr.inverse() * Ml).toTuple(),
            (True, ) * 6,
        )

        # 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),
            (False, False, True, True, True, False),
        )

        # Complement left foot
        result.append(prefix + "pose-left-foot-complement")
        problem.createTransformationConstraint2(
            result[-1],
            "",
            leftAnkle,
            Ml.toTuple(),
            (0, 0, 0, 0, 0, 0, 1),
            (True, True, False, False, False, True),
        )
        problem.setConstantRightHandSide(result[-1], False)

        return result
コード例 #3
0
def validateGazeConstraint(ps, q, whichArm):
    robot = ps.robot
    robot.setCurrentConfig(q)
    Mcamera = Transform(robot.getLinkPosition("talos/rgbd_optical_frame"))
    Mtarget = Transform(
        robot.getLinkPosition("talos/arm_" + whichArm + "_7_link"))
    z = (Mcamera.inverse() * Mtarget).translation[2]
    if z < .1: return False
    return True
コード例 #4
0
    def createStaticStabilityConstraint(self,
                                        prefix,
                                        comName,
                                        leftAnkle,
                                        rightAnkle,
                                        q0,
                                        maskCom=(True, ) * 3):
        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(), maskCom)

        # 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
コード例 #5
0
                           _) in zip(bag.read_messages(topics=["joints"]),
                                     bag.read_messages(topics=["chessboard"])):

    root_joint_rank = robot.rankInConfiguration["talos/root_joint"]
    q[root_joint_rank:root_joint_rank + 7] = [0, 0, 1, 0, 0, 0, 1]

    joints_name_value_tuple = zip(joint_states.name, joint_states.position)
    for name, value in joints_name_value_tuple:
        joint_name = "talos/" + name
        q[robot.rankInConfiguration[joint_name]] = value
    robot.setCurrentConfig(q)

    gripper = Transform(
        robot.getJointPosition("talos/gripper_left_base_link_joint"))
    camera = Transform(robot.getJointPosition("talos/rgbd_rgb_optical_joint"))
    fMe = gripper.inverse() * camera

    cMo = Transform([
        checkerboard_pose.pose.position.x,
        checkerboard_pose.pose.position.y,
        checkerboard_pose.pose.position.z,
        checkerboard_pose.pose.orientation.x,
        checkerboard_pose.pose.orientation.y,
        checkerboard_pose.pose.orientation.z,
        checkerboard_pose.pose.orientation.w,
    ])

    publishTransform(pub_cMo, cMo)
    publishTransform(pub_fMe, fMe)

    i += 1
コード例 #6
0
        0,0,0,0,0,0,1, # box
        ]
q_init = robot.getCurrentConfig()

ps.addPartialCom ("talos", ["talos/root_joint"])
ps.addPartialCom ("talos_box", ["talos/root_joint", "box/root_joint"])

ps.createStaticStabilityConstraints ("balance", half_sitting, "talos", ProblemSolver.FIXED_ON_THE_GROUND)
foot_placement = [ "balance/pose-left-foot", "balance/pose-right-foot" ]
foot_placement_complement = [ ]

robot.setCurrentConfig(half_sitting)
com_wf = np.array(ps.getPartialCom("talos"))
tf_la = Transform (robot.getJointPosition(robot.leftAnkle))
com_la = tf_la.inverse().transform(com_wf)

ps.createRelativeComConstraint ("com_talos_box", "talos_box", robot.leftAnkle, com_la.tolist(), (True, True, True))
ps.createRelativeComConstraint ("com_talos"    , "talos"    , robot.leftAnkle, com_la.tolist(), (True, True, True))

ps.createPositionConstraint ("gaze", "talos/rgbd_optical_joint", "box/root_joint", (0,0,0), (0,0,0), (True, True, False))

left_gripper_lock = []
right_gripper_lock = []
other_lock = ["talos/torso_1_joint"]
for n in robot.jointNames:
    s = robot.getJointConfigSize(n)
    r = robot.rankInConfiguration[n]
    if n.startswith ("talos/gripper_right"):
        ps.createLockedJoint(n, n, half_sitting[r:r+s])
        right_gripper_lock.append(n)
コード例 #7
0
## Project initial configuration on state 'placement'
res, q_init, error = graph.applyNodeConstraints ('placement', q1)
q2 = q1 [::]
q2 [7] = .2

## Project goal configuration on state 'placement'
res, q_goal, error = graph.applyNodeConstraints ('placement', q2)

## Define manipulation planning problem
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)

# v = vf.createViewer ()
# pp = PathPlayer (v)
# v (q1)

## Build relative position of the ball with respect to the gripper
for i in range (100):
  q = robot.shootRandomConfig ()
  res,q3,err = graph.generateTargetConfig ('grasp-ball', q_init, q)
  if res and robot.isConfigValid (q3): break;

if res:
  robot.setCurrentConfig (q3)
  gripperPose = Transform (robot.getJointPosition (gripperName))
  ballPose = Transform (robot.getJointPosition (ballName))
  gripperGraspsBall = gripperPose.inverse () * ballPose
  gripperAboveBall = Transform (gripperGraspsBall)
  gripperAboveBall.translation [2] += .1
コード例 #8
0
## Project initial configuration on state 'placement'
res, q_init, error = ps.client.manipulation.problem.applyConstraints \
                     (graph.nodes ['placement'], q1)
q2 = q1 [::]
q2 [7] = .2

## Project goal configuration on state 'placement'
res, q_goal, error = ps.client.manipulation.problem.applyConstraints \
                     (graph.nodes ['placement'], q2)

## Define manipulation planning problem
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathValidation ("Dichotomy", 0)

pp = PathPlayer (ps.client.basic, r)

## Build relative position of the ball with respect to the gripper
for i in range (100):
  q = robot.shootRandomConfig ()
  res,q3,err = graph.generateTargetConfig ('grasp-ball', q_init, q)
  if res and robot.isConfigValid (q3): break;

if res:
  robot.setCurrentConfig (q3)
  gripperPose = Transform (robot.getLinkPosition (gripperName))
  ballPose = Transform (robot.getLinkPosition (ballName))
  gripperAboveBall = gripperPose.inverse () * ballPose
  gripperAboveBall.translation [2] += .1
コード例 #9
0

# END INIT CHESSBOARD AND CAMERA PARAM AND FUNCTIONS

ps.addPartialCom("talos", ["talos/root_joint"])
ps.addPartialCom("talos_mire", ["talos/root_joint", "mire/root_joint"])

ps.createStaticStabilityConstraints("balance", half_sitting, "talos",
                                    ProblemSolver.FIXED_ON_THE_GROUND)
foot_placement = ["balance/pose-left-foot", "balance/pose-right-foot"]
foot_placement_complement = []

robot.setCurrentConfig(half_sitting)
com_wf = np.array(ps.getPartialCom("talos"))
tf_la = Transform(robot.getJointPosition(robot.leftAnkle))
com_la = tf_la.inverse().transform(com_wf)

ps.createRelativeComConstraint("com_talos_mire", "talos_mire", robot.leftAnkle,
                               com_la.tolist(), (True, True, True))
ps.createRelativeComConstraint("com_talos", "talos", robot.leftAnkle,
                               com_la.tolist(), (True, True, True))

left_gripper_lock = []
right_gripper_lock = []
head_lock = []
other_lock = ["talos/torso_1_joint"]
for n in robot.jointNames:
    s = robot.getJointConfigSize(n)
    r = robot.rankInConfiguration[n]
    if n.startswith("talos/gripper_right"):
        ps.createLockedJoint(n, n, half_sitting[r:r + s])
コード例 #10
0
## Project initial configuration on state 'placement'
res, q_init, error = ps.client.manipulation.problem.applyConstraints \
                     (graph.nodes ['placement'], q1)
q2 = q1[::]
q2[7] = .2

## Project goal configuration on state 'placement'
res, q_goal, error = ps.client.manipulation.problem.applyConstraints \
                     (graph.nodes ['placement'], q2)

## Define manipulation planning problem
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.selectPathValidation("Dichotomy", 0)

pp = PathPlayer(ps.client.basic, r)

## Build relative position of the ball with respect to the gripper
for i in range(100):
    q = robot.shootRandomConfig()
    res, q3, err = graph.generateTargetConfig('grasp-ball', q_init, q)
    if res and robot.isConfigValid(q3): break

if res:
    robot.setCurrentConfig(q3)
    gripperPose = Transform(robot.getLinkPosition(gripperName))
    ballPose = Transform(robot.getLinkPosition(ballName))
    gripperAboveBall = gripperPose.inverse() * ballPose
    gripperAboveBall.translation[2] += .1