def handleFixedLinkFromRobotPoseConstraint(self, c, fields): bodyId = self.bodyNameToId[c.linkName] pose = np.asarray(fields.poses[c.poseName], dtype=float) pointInBodyFrame = np.zeros(3) tolerance = np.radians(c.angleToleranceInDegrees) tspan = np.asarray(c.tspan, dtype=float) bodyToWorld = self.computeBodyToWorld(pose, c.linkName, pointInBodyFrame) bodyPos = transformUtils.transformations.translation_from_matrix( bodyToWorld) bodyQuat = transformUtils.transformations.quaternion_from_matrix( bodyToWorld) lowerBound = bodyPos + c.lowerBound upperBound = bodyPos + c.upperBound pc = pydrakeik.WorldPositionConstraint(self.rigidBodyTree, bodyId, pointInBodyFrame, lowerBound, upperBound, tspan) qc = pydrakeik.WorldQuatConstraint(self.rigidBodyTree, bodyId, bodyQuat, tolerance, tspan) return [pc, qc]
def setup_scene(rbt): AddFlatTerrainToWorld(rbt) instances = [] num_objects = np.random.randint(10) + 1 for i in range(num_objects): object_ind = np.random.randint(len(objects.keys())) pose = np.zeros(6) pose[0:2] = (np.random.random(2) - 0.5) * 0.05 pose[2] = np.random.random() * 0.1 + 0.05 pose[3:6] = np.random.random(3) * np.pi * 2. object_init_frame = RigidBodyFrame("object_init_frame", rbt.world(), pose[0:3], pose[3:6]) object_path = objects[objects.keys()[object_ind]]["model_path"] if object_path.split(".")[-1] == "urdf": AddModelInstanceFromUrdfFile(object_path, FloatingBaseType.kRollPitchYaw, object_init_frame, rbt) else: AddModelInstancesFromSdfString( open(object_path).read(), FloatingBaseType.kRollPitchYaw, object_init_frame, rbt) instances.append({ "class": objects.keys()[object_ind], "pose": pose.tolist() }) rbt.compile() # Project arrangement to nonpenetration with IK constraints = [] constraints.append( ik.MinDistanceConstraint(model=rbt, min_distance=0.01, active_bodies_idx=list(), active_group_names=set())) for body_i in range(2, 1 + num_objects): constraints.append( ik.WorldPositionConstraint(model=rbt, body=body_i, pts=np.array([0., 0., 0.]), lb=np.array([-0.5, -0.5, 0.0]), ub=np.array([0.5, 0.5, 0.5]))) q0 = np.zeros(rbt.get_num_positions()) options = ik.IKoptions(rbt) options.setDebug(True) options.setMajorIterationsLimit(10000) options.setIterationsLimit(100000) results = ik.InverseKin(rbt, q0, q0, constraints, options) qf = results.q_sol[0] info = results.info[0] print "Projected with info %d" % info return qf, instances
def onFrameModified(obj): pos, quat = transformUtils.poseFromTransform(obj.transform) pc = ik.WorldPositionConstraint(robot, leftHandBodyId, np.zeros((3, )), pos, pos, tspan) qc = ik.WorldQuatConstraint(robot, leftHandBodyId, quat, 0.0, tspan) solveAndDraw([qsc, pc, qc] + footConstraints)
def ComputeTargetPosture(self, x_seed, target_manipuland_pose, backoff_distance=0.0): q_des_full = np.zeros(self.nq) q_des_full[self.nq - 3:] = target_manipuland_pose desired_positions = self.transform_grasp_points_manipuland(q_des_full) desired_normals = self.transform_grasp_normals_manipuland(q_des_full) desired_positions -= backoff_distance * desired_normals # back off a bit desired_angles = [ math.atan2(desired_normals[1, k], desired_normals[0, k]) for k in range(desired_normals.shape[1]) ] constraints = [] # Constrain the fingertips to lie on the grasp points, # facing approximately along the grasp angles. for i in range(len(self.grasp_points)): constraints.append( ik.WorldPositionConstraint( self.hand, self.finger_link_indices[self.grasp_finger_assignments[i] [0]], self.grasp_finger_assignments[i][1], desired_positions[:, i] - 0.01, # lb desired_positions[:, i] + 0.01) # ub ) constraints.append( ik.WorldEulerConstraint( self.hand, self.finger_link_indices[self.grasp_finger_assignments[i] [0]], [-0.01, -0.01, desired_angles[i] - 1.0], # lb [0.01, 0.01, desired_angles[i] + 1.0]) # ub ) posture_constraint = ik.PostureConstraint(self.hand) # Disambiguate the continuous rotation joint angle choices # for the hand. posture_constraint.setJointLimits(np.arange(self.nq - 3), -np.ones(self.nq - 3) * math.pi, np.ones(self.nq - 3) * math.pi) # Constrain the manipuland to be in the target position. posture_constraint.setJointLimits( [self.nq - 3, self.nq - 2, self.nq - 1], target_manipuland_pose - 0.01, target_manipuland_pose + 0.01) constraints.append(posture_constraint) options = ik.IKoptions(self.hand) results = ik.InverseKin(self.hand, x_seed[0:self.nq], self.x_nom[0:self.nq], constraints, options) return results.q_sol[0], results.info[0]
def plan_ee_configuration(rbt, q0, qseed, ee_pose, ee_body, ee_point, allow_collision=True, active_bodies_idx=list(), euler_limits=0.3): ''' Performs IK for a single point in time to get the Kuka's gripper to a specified pose in space. ''' nq = rbt.get_num_positions() controlled_joint_names = [ "iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4", "iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7" ] free_config_inds, constrained_config_inds = \ kuka_utils.extract_position_indices(rbt, controlled_joint_names) # Assemble IK constraints constraints = [] if not allow_collision: constraints.append( ik.MinDistanceConstraint(model=rbt, min_distance=1E-6, active_bodies_idx=active_bodies_idx, active_group_names=set())) # Constrain the non-searched-over joints posture_constraint = ik.PostureConstraint(rbt) posture_constraint.setJointLimits(constrained_config_inds, q0[constrained_config_inds] - 0.001, q0[constrained_config_inds] + 0.001) constraints.append(posture_constraint) # Constrain the ee frame to lie on the target point # facing in the target orientation constraints.append( ik.WorldPositionConstraint(rbt, ee_body, ee_point, ee_pose[0:3] - 0.01, ee_pose[0:3] + 0.01)) constraints.append( ik.WorldEulerConstraint(rbt, ee_body, ee_pose[3:6] - euler_limits, ee_pose[3:6] + euler_limits)) options = ik.IKoptions(rbt) results = ik.InverseKin(rbt, q0, qseed, constraints, options) #print "plan ee config info %d" % results.info[0] return results.q_sol[0], results.info[0]
def plan_grasping_configuration(rbt, q0, target_ee_pose): ''' Performs IK for a single point in time to get the Kuka's gripper to a specified pose in space. ''' nq = rbt.get_num_positions() q_des_full = np.zeros(nq) controlled_joint_names = [ "iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4", "iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7" ] free_config_inds, constrained_config_inds = \ kuka_utils.extract_position_indices(rbt, controlled_joint_names) # Assemble IK constraints constraints = [] # Constrain the non-searched-over joints posture_constraint = ik.PostureConstraint(rbt) posture_constraint.setJointLimits(constrained_config_inds, q0[constrained_config_inds] - 0.01, q0[constrained_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain the ee frame to lie on the target point # facing in the target orientation ee_frame = rbt.findFrame("iiwa_frame_ee").get_frame_index() constraints.append( ik.WorldPositionConstraint(rbt, ee_frame, np.zeros( (3, 1)), target_ee_pose[0:3] - 0.01, target_ee_pose[0:3] + 0.01)) constraints.append( ik.WorldEulerConstraint(rbt, ee_frame, target_ee_pose[3:6] - 0.01, target_ee_pose[3:6] + 0.01)) options = ik.IKoptions(rbt) results = ik.InverseKin(rbt, q0, q0, constraints, options) print results.q_sol, "info %d" % results.info[0] return results.q_sol[0], results.info[0]
def runIK(self, pose_mat): # Set up constraints constraints = list() xyz = transf.translation_from_matrix(pose_mat) pos_delta = self.config["pose_delta"] constraints.append( ik.WorldPositionConstraint( self.robot, self.robot.bodies["iiwa_tool_frame"], # Frame of reference np.array([0.0, 0.0, 0.0]), # Point to constrain (in frame of reference) xyz - pos_delta, # Lower bound xyz + pos_delta # Upper bound )) rpy = np.array(transf.euler_from_matrix(pose_mat)) quat_tol = self.config["quat_tol"] constraints.append( ik.WorldEulerConstraint( self.robot, self.robot.bodies["iiwa_tool_frame"], # Frame of reference rpy - quat_tol, # Lower bound rpy + quat_tol # Upper bound )) # Run the IK q_seed = self.robot.getZeroConfiguration() options = ik.IKoptions(self.robot) result = ik.InverseKin(self.robot, q_seed, q_seed, constraints, options) if result.info[0] == 1: return result.q_sol[0][2:], True else: return [], False
def testIkPlan(): ikPlanner = robotSystem.ikPlanner constraints = buildConstraints() poses = ce.getPlanPoses(constraints, ikPlanner) global robot robot = loadRigidBodyTree(getIkUrdfFilename()) tspan = np.array([1.0, 1.0]) handLinkNames = [ikPlanner.getHandLink('left'), ikPlanner.getHandLink('right')] footLinkNames = [robotSystem.directorConfig['leftFootLink'], robotSystem.directorConfig['rightFootLink']] leftHandBodyId = robot.FindBody(str(handLinkNames[0])).get_body_index() rightHandBodyId = robot.FindBody(str(handLinkNames[1])).get_body_index() leftFootBodyId = robot.FindBody(str(footLinkNames[0])).get_body_index() rightFootBodyId = robot.FindBody(str(footLinkNames[1])).get_body_index() lfootContactPts, rfootContactPts = getFootContactPoints() #lfootContactPts, rfootContactPts = = robotSystem.footstepsDriver.getContactPts() for i in xrange(robot.get_num_bodies()): body = robot.get_body(i) print i, body.get_name() assert robot.FindBodyIndex(body.get_name()) == i q = np.zeros(robot.get_num_positions()) v = np.zeros(robot.get_num_velocities()) kinsol = robot.doKinematics(q,v) t = robot.relativeTransform(kinsol, robot.FindBodyIndex('world'), robot.FindBodyIndex(str(footLinkNames[0]))) tt = transformUtils.getTransformFromNumpy(t) pos = transformUtils.transformations.translation_from_matrix(t) quat = transformUtils.transformations.quaternion_from_matrix(t) footConstraints = [] for linkName in footLinkNames: t = robotSystem.robotStateModel.getLinkFrame(linkName) pos, quat = transformUtils.poseFromTransform(t) if False and linkName == footLinkNames[0]: pc = pydrakeik.WorldPositionConstraint(robot, robot.findLink(str(linkName)).get_body_index(), np.zeros((3,)), pos + [-10,-10,0], pos+[10,10,0], tspan) else: pc = pydrakeik.WorldPositionConstraint(robot, robot.findLink(str(linkName)).get_body_index(), np.zeros((3,)), pos, pos, tspan) qc = pydrakeik.WorldQuatConstraint(robot, robot.findLink(str(linkName)).get_body_index(), quat, 0.01, tspan) footConstraints.append(pc) footConstraints.append(qc) qsc = pydrakeik.QuasiStaticConstraint(robot, tspan) qsc.setActive(True) qsc.setShrinkFactor(1.0) qsc.addContact([leftFootBodyId], lfootContactPts.transpose()) qsc.addContact([rightFootBodyId], rfootContactPts.transpose()) #q_seed = robot.getZeroConfiguration() #q_seed = robotSystem.robotStateJointController.getPose('q_nom').copy() # todo, joint costs, and other options global options options = pydrakeik.IKoptions(robot) jointIndexMap = np.zeros(robotSystem.robotStateJointController.numberOfJoints) drakePositionNames = [robot.get_position_name(i) for i in xrange(robot.get_num_positions())] for i, name in enumerate(robotSystem.robotStateJointController.jointNames): jointIndexMap[i] = drakePositionNames.index(name) jointIndexMap = list(jointIndexMap) print jointIndexMap q_seed = np.zeros(robot.get_num_positions()) q_seed[jointIndexMap] = robotSystem.robotStateJointController.getPose('q_nom').copy() # setup costs costs = np.ones(robot.get_num_positions()) groupCosts = [ ('Left Leg', 1e3), ('Right Leg', 1e3), ('Left Arm', 1), ('Right Arm', 1), ('Back', 1e4), ('Neck', 1e6), ] for groupName, costValue in groupCosts: names = robotSystem.ikPlanner.getJointGroup(groupName) inds = [drakePositionNames.index(name) for name in names] print costValue, names costs[inds] = costValue costs[drakePositionNames.index('base_x')] = 0 costs[drakePositionNames.index('base_y')] = 0 costs[drakePositionNames.index('base_z')] = 0 costs[drakePositionNames.index('base_roll')] = 1e3 costs[drakePositionNames.index('base_pitch')] = 1e3 costs[drakePositionNames.index('base_yaw')] = 0 print costs options.setQ(np.diag(costs)) def solveAndDraw(constraints): #print q_seed.shape global results global constraintList constraintList = constraints results = pydrakeik.InverseKin(robot, q_seed, q_seed, constraints, options) pose = results.q_sol[0] #print results.info[0] pose = pose[jointIndexMap] robotSystem.robotStateJointController.setPose('pose_end', pose) def onFrameModified(obj): pos, quat = transformUtils.poseFromTransform(obj.transform) pc = pydrakeik.WorldPositionConstraint(robot, leftHandBodyId, np.zeros((3,)), pos, pos, tspan) qc = pydrakeik.WorldQuatConstraint(robot, leftHandBodyId, quat, 0.01, tspan) solveAndDraw([qsc, pc, qc] + footConstraints) #solveAndDraw([pc]) frameObj = vis.updateFrame(robotSystem.robotStateModel.getLinkFrame(ikPlanner.getHandLink('left')), 'left hand frame') frameObj.setProperty('Edit', True) frameObj.setProperty('Scale', 0.2) frameObj.connectFrameModified(onFrameModified)
hand_frame_id = robot.findFrame("r_hand_frame").get_frame_index() base_body_id = robot.FindBody('base_footprint').get_body_index() constraints = [ # These three constraints ensure that the base of the robot is # at z = 0 and has no pitch or roll. Instead of directly # constraining orientation, we just require that the points at # [0, 0, 0], [1, 0, 0], and [0, 1, 0] in the robot's base's # frame must all be at z = 0 in world frame. # We don't care about the x or y position of the robot's base, # so we use NaN values to tell the IK solver not to apply a # constraint along those dimensions. This is equivalent to # placing a lower bound of -Inf and an upper bound of +Inf along # those axes. ik.WorldPositionConstraint(robot, base_body_id, np.array([0.0, 0.0, 0.0]), np.array([np.nan, np.nan, 0.0]), np.array([np.nan, np.nan, 0.0])), ik.WorldPositionConstraint(robot, base_body_id, np.array([1.0, 0.0, 0.0]), np.array([np.nan, np.nan, 0.0]), np.array([np.nan, np.nan, 0.0])), ik.WorldPositionConstraint(robot, base_body_id, np.array([0.0, 1.0, 0.0]), np.array([np.nan, np.nan, 0.0]), np.array([np.nan, np.nan, 0.0])), # This constraint exactly specifies the desired position of the # hand frame we defined earlier. ik.WorldPositionConstraint(robot, hand_frame_id, np.array([0.0, 0.0, 0.0]), np.array([0.5, 0.0, 0.6]), np.array([0.5, 0.0, 0.6])), # And this specifies the orientation of that frame ik.WorldEulerConstraint(robot, hand_frame_id, np.array([0.0, 0.0, 0.0]),
def plan_grasping_trajectory(rbt, q0, target_reach_pose, target_grasp_pose, n_knots, reach_time, grasp_time): ''' Solves IK at a series of sample times (connected with a cubic spline) to generate a trajectory to bring the Kuka from an initial pose q0 to a final end effector pose in the specified time, using the specified number of knot points. Uses an intermediate pose reach_pose as an intermediate target to hit at the knot point closest to reach_time. See http://drake.mit.edu/doxygen_cxx/rigid__body__ik_8h.html for the "inverseKinTraj" entry. At the moment, the Python binding for this function uses "inverseKinTrajSimple" -- i.e., it doesn't return derivatives. ''' nq = rbt.get_num_positions() q_des_full = np.zeros(nq) # Create knot points ts = np.linspace(0., grasp_time, n_knots) # Figure out the knot just before reach time reach_start_index = np.argmax(ts >= reach_time) - 1 controlled_joint_names = [ "iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4", "iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7" ] free_config_inds, constrained_config_inds = \ kuka_utils.extract_position_indices(rbt, controlled_joint_names) # Assemble IK constraints constraints = [] # Constrain the non-searched-over joints for all time all_tspan = np.array([0., grasp_time]) posture_constraint = ik.PostureConstraint(rbt, all_tspan) posture_constraint.setJointLimits(constrained_config_inds, q0[constrained_config_inds] - 0.01, q0[constrained_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain all joints to be the initial posture at the start time start_tspan = np.array([0., 0.]) posture_constraint = ik.PostureConstraint(rbt, start_tspan) posture_constraint.setJointLimits(free_config_inds, q0[free_config_inds] - 0.01, q0[free_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain the ee frame to lie on the target point # facing in the target orientation in between the # reach and final times ee_frame = rbt.findFrame("iiwa_frame_ee").get_frame_index() for i in range(reach_start_index, n_knots): this_tspan = np.array([ts[i], ts[i]]) interp = float(i - reach_start_index) / (n_knots - reach_start_index) target_pose = (1. - interp) * target_reach_pose + interp * target_grasp_pose constraints.append( ik.WorldPositionConstraint(rbt, ee_frame, np.zeros((3, 1)), target_pose[0:3] - 0.01, target_pose[0:3] + 0.01, tspan=this_tspan)) constraints.append( ik.WorldEulerConstraint(rbt, ee_frame, target_pose[3:6] - 0.05, target_pose[3:6] + 0.05, tspan=this_tspan)) # Seed and nom are both the initial repeated for the # # of knot points q_seed = np.tile(q0, [1, n_knots]) q_nom = np.tile(q0, [1, n_knots]) options = ik.IKoptions(rbt) # Set bounds on initial and final velocities zero_velocity = np.zeros(rbt.get_num_velocities()) options.setqd0(zero_velocity, zero_velocity) options.setqdf(zero_velocity, zero_velocity) results = ik.InverseKinTraj(rbt, ts, q_seed, q_nom, constraints, options) qtraj = PiecewisePolynomial.Pchip(ts, np.vstack(results.q_sol).T, True) print "IK returned a solution with info %d" % results.info[0] print "(Info 1 is good, other values are dangerous)" return qtraj, results.info[0]
import os import numpy as np import pydrake from pydrake.solvers import ik robot = pydrake.rbtree.RigidBodyTree( os.path.join(pydrake.getDrakePath(), "examples/PR2/pr2.urdf")) # Make sure attribute access works on bodies assert robot.bodies[0].linkname == "world" constraints = [ ik.WorldPositionConstraint(robot, 1, np.zeros((3, )), np.array([0, 0, 1.0]), np.array([0, 0, 1.0])), ik.WorldPositionConstraint( robot, robot.findLink("r_gripper_palm_link").body_index, np.zeros((3, )), np.array([0.5, 0, 1.5]), np.array([0.5, 0, 1.5])), ] q_seed = robot.getZeroConfiguration() options = ik.IKoptions(robot) results = ik.inverseKinSimple(robot, q_seed, q_seed, constraints, options) print repr(results.q_sol)
def plan_ee_trajectory(rbt, q0, traj_seed, ee_times, ee_poses, ee_body, ee_point, n_knots, start_time=0., avoid_collision_tspans=[], active_bodies_idx=list()): nq = rbt.get_num_positions() # Create knot points ts = np.linspace(0., ee_times[-1], n_knots) # Insert one more for each ee time if not already in there for t in ee_times: if t not in ts: ts = np.insert(ts, np.argmax(ts >= t), t) print "times ", ts, " for times ", ee_times controlled_joint_names = [ "iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4", "iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7" ] free_config_inds, constrained_config_inds = \ kuka_utils.extract_position_indices(rbt, controlled_joint_names) # Assemble IK constraints constraints = [] for tspan in avoid_collision_tspans: constraints.append( ik.MinDistanceConstraint(model=rbt, min_distance=1E-6, active_bodies_idx=active_bodies_idx, active_group_names=set(), tspan=tspan)) print "active for ", tspan # Make starting constraint sensible if it's out of joint limits q0 = np.clip(q0, rbt.joint_limit_min, rbt.joint_limit_max) # Constrain the non-searched-over joints for all time all_tspan = np.array([0., ts[-1]]) posture_constraint = ik.PostureConstraint(rbt, all_tspan) posture_constraint.setJointLimits(constrained_config_inds, q0[constrained_config_inds] - 0.05, q0[constrained_config_inds] + 0.05) constraints.append(posture_constraint) # Constrain all joints to be the initial posture at the start time start_tspan = np.array([0., 0.]) posture_constraint = ik.PostureConstraint(rbt, start_tspan) posture_constraint.setJointLimits(free_config_inds, q0[free_config_inds] - 0.01, q0[free_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain the ee frame to lie on the target point # facing in the target orientation in between the # reach and final times) for t, pose in zip(ee_times, ee_poses): this_tspan = np.array([t - 0.01, t + 0.01]) constraints.append( ik.WorldPositionConstraint(rbt, ee_body, ee_point, pose[0:3] - 0.01, pose[0:3] + 0.01, tspan=this_tspan)) constraints.append( ik.WorldEulerConstraint(rbt, ee_body, pose[3:6] - 0.05, pose[3:6] + 0.05, tspan=this_tspan)) # Seed and nom are both the initial repeated for the # # of knot points q_seed = np.hstack([traj_seed.value(t) for t in ts]) q_nom = np.tile(q0, [ts.shape[0], 1]).T options = ik.IKoptions(rbt) # Set bounds on initial and final velocities zero_velocity = np.zeros(rbt.get_num_velocities()) options.setqd0(zero_velocity, zero_velocity) options.setqdf(zero_velocity, zero_velocity) Q = np.eye(q0.shape[0], q0.shape[0]) Q[free_config_inds[-2:], free_config_inds[-2:]] *= 1 options.setQ(Q) options.setQv(np.eye(q0.shape[0], q0.shape[0]) * 10) results = ik.InverseKinTraj(rbt, ts, q_nom, q_seed, constraints, options) qtraj = PiecewisePolynomial.Pchip(ts + start_time, np.vstack(results.q_sol).T, True) print "IK returned a solution with info %d" % results.info[0] return qtraj, results.info[0], results.q_sol
def testIkPlan(): ikPlanner = robotSystem.ikPlanner ikPlanner.pushToMatlab = False constraints = buildConstraints() poses = ce.getPlanPoses(constraints, ikPlanner) robot = loadRigidBodyTree(getIkUrdfFilename()) #for i, body in enumerate(robot.bodies): # print i, body.linkname tspan = np.array([1.0, 1.0]) handLinkNames = [ ikPlanner.getHandLink('left'), ikPlanner.getHandLink('right') ] footLinkNames = [ robotSystem.directorConfig['leftFootLink'], robotSystem.directorConfig['rightFootLink'] ] leftHandBodyId = robot.findLink(str(handLinkNames[0])).body_index rightHandBodyId = robot.findLink(str(handLinkNames[1])).body_index leftFootBodyId = robot.findLink(str(footLinkNames[0])).body_index rightFootBodyId = robot.findLink(str(footLinkNames[1])).body_index lfootContactPts, rfootContactPts = robotSystem.footstepsDriver.getContactPts( ) footConstraints = [] for linkName in footLinkNames: t = robotSystem.robotStateModel.getLinkFrame(linkName) pos, quat = transformUtils.poseFromTransform(t) pc = ik.WorldPositionConstraint( robot, robot.findLink(str(linkName)).body_index, np.zeros((3, )), pos, pos, tspan) qc = ik.WorldQuatConstraint(robot, robot.findLink(str(linkName)).body_index, quat, 0.0, tspan) footConstraints.append(pc) footConstraints.append(qc) qsc = ik.QuasiStaticConstraint(robot, tspan) qsc.setActive(True) qsc.setShrinkFactor(0.5) qsc.addContact([leftFootBodyId], lfootContactPts.transpose()) qsc.addContact([rightFootBodyId], rfootContactPts.transpose()) #q_seed = robot.getZeroConfiguration() q_seed = robotSystem.robotStateJointController.getPose('q_nom').copy() # todo, joint costs, and other options options = ik.IKoptions(robot) def solveAndDraw(constraints): results = ik.inverseKinSimple(robot, q_seed, q_seed, constraints, options) pose = results.q_sol robotSystem.robotStateJointController.setPose('pose_end', pose) def onFrameModified(obj): pos, quat = transformUtils.poseFromTransform(obj.transform) pc = ik.WorldPositionConstraint(robot, leftHandBodyId, np.zeros((3, )), pos, pos, tspan) qc = ik.WorldQuatConstraint(robot, leftHandBodyId, quat, 0.0, tspan) solveAndDraw([qsc, pc, qc] + footConstraints) frameObj = vis.updateFrame( robotSystem.robotStateModel.getLinkFrame( ikPlanner.getHandLink('left')), 'left hand frame') frameObj.setProperty('Edit', True) frameObj.setProperty('Scale', 0.2) frameObj.connectFrameModified(onFrameModified)