Variables
demo_tsid_talos_gripper_closed_kinematic_chain Namespace Reference

Variables

 actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
 
 aEE = np.zeros(6)
 
 amp = np.array([0.0, 0.04, 0.0])
 
 ContactTwoFramePositionsFingertipBottomAxis
 
 data = invdyn.data()
 
int DISPLAY_N = 25
 
float dt = 0.001
 
 dv = invdyn.getAccelerations(sol)
 
 filename = str(Path(__file__).resolve().parent)
 
string fingertip_name = "gripper_left_fingertip_3_link"
 
 fingertipPositionTask
 
 H_fingertip_ref = robot.framePosition(invdyn.data(), model.getFrameId(fingertip_name))
 
 HQPData = invdyn.computeProblemData(t, q, v)
 
int i = 0
 
 invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
 
float kp_ee = 10.0
 
float kp_posture = 10.0
 
int LINE_WIDTH = 60
 
 linewidth
 
 loadModel
 
 model = robot.model()
 
int N_SIMULATION = 6000
 
 offset = sampleFingertipPosition.pos()
 
 open
 
string path = filename + "../../tsid_demo_closed_kinematic_chain/models/talos_gripper"
 
 pEE = offset.copy()
 
 postureTask = tsid.TaskJointPosture("task-posture", robot)
 
 precision
 
int PRINT_N = 500
 
 q = np.zeros(robot.nq)
 
 q_ref = q
 
 robot = tsid.RobotWrapper(urdf, vector, False)
 
 robot_display = pin.RobotWrapper.BuildFromURDF(urdf, [path])
 
 sampleFingertipPosition = trajFingertipPosition.computeNext()
 
 samplePosture = trajPosture.computeNext()
 
 sol = solver.solve(HQPData)
 
 solver = tsid.SolverHQuadProgFast("qp solver")
 
 suppress
 
float t = 0.0
 
 tau = invdyn.getActuatorForces(sol)
 
 tau_max = model.effortLimit[-robot.na :]
 
 tau_min = -tau_max
 
 time_spent = time.time() - time_start
 
 time_start = time.time()
 
 trajFingertipPosition
 
 trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
 
 True
 
int two_pi_f = 2 * np.pi * np.array([0.0, 0.5, 0.0])
 
 two_pi_f_amp = np.multiply(two_pi_f, amp)
 
 two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
 
int twoFramesContact_Kp = 300
 
int twoFramesContact_w_forceRef = 1e-5
 
string urdf = path + "../../tsid_demo_closed_kinematic_chain/urdf/talos_gripper_half.urdf"
 
 v = np.zeros(robot.nv)
 
float v_mean = v + 0.5 * dt * dv
 
 vector = pin.StdVec_StdString()
 
 vEE = np.zeros(6)
 
 viz
 
float w_ee = 1.0
 
int w_posture = 1e-3
 

Detailed Description

Simple demo of usage of ContactTwoFramePositions in TSID
Make the Talos gripper model works with closed kinematic chains
(c) MIPT

Variable Documentation

◆ actuationBoundsTask

demo_tsid_talos_gripper_closed_kinematic_chain.actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)

◆ aEE

demo_tsid_talos_gripper_closed_kinematic_chain.aEE = np.zeros(6)

◆ amp

demo_tsid_talos_gripper_closed_kinematic_chain.amp = np.array([0.0, 0.04, 0.0])

◆ ContactTwoFramePositionsFingertipBottomAxis

demo_tsid_talos_gripper_closed_kinematic_chain.ContactTwoFramePositionsFingertipBottomAxis
Initial value:
1 = tsid.ContactTwoFramePositions(
2  "contact-two-frame-positions-fingertip-bottom-axis",
3  robot,
4  "gripper_left_motor_single_link_ckc_axis",
5  "gripper_left_fingertip_3_link_ckc_axis",
6  -1000,
7  1000,
8 )

Definition at line 92 of file demo_tsid_talos_gripper_closed_kinematic_chain.py.

◆ data

demo_tsid_talos_gripper_closed_kinematic_chain.data = invdyn.data()

◆ DISPLAY_N

int demo_tsid_talos_gripper_closed_kinematic_chain.DISPLAY_N = 25

◆ dt

float demo_tsid_talos_gripper_closed_kinematic_chain.dt = 0.001

◆ dv

demo_tsid_talos_gripper_closed_kinematic_chain.dv = invdyn.getAccelerations(sol)

◆ filename

demo_tsid_talos_gripper_closed_kinematic_chain.filename = str(Path(__file__).resolve().parent)

◆ fingertip_name

string demo_tsid_talos_gripper_closed_kinematic_chain.fingertip_name = "gripper_left_fingertip_3_link"

◆ fingertipPositionTask

demo_tsid_talos_gripper_closed_kinematic_chain.fingertipPositionTask
Initial value:
1 = tsid.TaskSE3Equality(
2  "task-fingertip-position", robot, fingertip_name
3 )

Definition at line 72 of file demo_tsid_talos_gripper_closed_kinematic_chain.py.

◆ H_fingertip_ref

demo_tsid_talos_gripper_closed_kinematic_chain.H_fingertip_ref = robot.framePosition(invdyn.data(), model.getFrameId(fingertip_name))

◆ HQPData

demo_tsid_talos_gripper_closed_kinematic_chain.HQPData = invdyn.computeProblemData(t, q, v)

◆ i

int demo_tsid_talos_gripper_closed_kinematic_chain.i = 0

◆ invdyn

demo_tsid_talos_gripper_closed_kinematic_chain.invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)

◆ kp_ee

float demo_tsid_talos_gripper_closed_kinematic_chain.kp_ee = 10.0

◆ kp_posture

float demo_tsid_talos_gripper_closed_kinematic_chain.kp_posture = 10.0

◆ LINE_WIDTH

int demo_tsid_talos_gripper_closed_kinematic_chain.LINE_WIDTH = 60

◆ linewidth

demo_tsid_talos_gripper_closed_kinematic_chain.linewidth

◆ loadModel

demo_tsid_talos_gripper_closed_kinematic_chain.loadModel

◆ model

demo_tsid_talos_gripper_closed_kinematic_chain.model = robot.model()

◆ N_SIMULATION

int demo_tsid_talos_gripper_closed_kinematic_chain.N_SIMULATION = 6000

◆ offset

demo_tsid_talos_gripper_closed_kinematic_chain.offset = sampleFingertipPosition.pos()

◆ open

demo_tsid_talos_gripper_closed_kinematic_chain.open

◆ path

string demo_tsid_talos_gripper_closed_kinematic_chain.path = filename + "../../tsid_demo_closed_kinematic_chain/models/talos_gripper"

◆ pEE

demo_tsid_talos_gripper_closed_kinematic_chain.pEE = offset.copy()

◆ postureTask

demo_tsid_talos_gripper_closed_kinematic_chain.postureTask = tsid.TaskJointPosture("task-posture", robot)

◆ precision

demo_tsid_talos_gripper_closed_kinematic_chain.precision

◆ PRINT_N

int demo_tsid_talos_gripper_closed_kinematic_chain.PRINT_N = 500

◆ q

demo_tsid_talos_gripper_closed_kinematic_chain.q = np.zeros(robot.nq)

◆ q_ref

demo_tsid_talos_gripper_closed_kinematic_chain.q_ref = q

◆ robot

demo_tsid_talos_gripper_closed_kinematic_chain.robot = tsid.RobotWrapper(urdf, vector, False)

◆ robot_display

demo_tsid_talos_gripper_closed_kinematic_chain.robot_display = pin.RobotWrapper.BuildFromURDF(urdf, [path])

◆ sampleFingertipPosition

demo_tsid_talos_gripper_closed_kinematic_chain.sampleFingertipPosition = trajFingertipPosition.computeNext()

◆ samplePosture

demo_tsid_talos_gripper_closed_kinematic_chain.samplePosture = trajPosture.computeNext()

◆ sol

demo_tsid_talos_gripper_closed_kinematic_chain.sol = solver.solve(HQPData)

◆ solver

demo_tsid_talos_gripper_closed_kinematic_chain.solver = tsid.SolverHQuadProgFast("qp solver")

◆ suppress

demo_tsid_talos_gripper_closed_kinematic_chain.suppress

◆ t

float demo_tsid_talos_gripper_closed_kinematic_chain.t = 0.0

◆ tau

demo_tsid_talos_gripper_closed_kinematic_chain.tau = invdyn.getActuatorForces(sol)

◆ tau_max

demo_tsid_talos_gripper_closed_kinematic_chain.tau_max = model.effortLimit[-robot.na :]

◆ tau_min

demo_tsid_talos_gripper_closed_kinematic_chain.tau_min = -tau_max

◆ time_spent

demo_tsid_talos_gripper_closed_kinematic_chain.time_spent = time.time() - time_start

◆ time_start

demo_tsid_talos_gripper_closed_kinematic_chain.time_start = time.time()

◆ trajFingertipPosition

demo_tsid_talos_gripper_closed_kinematic_chain.trajFingertipPosition
Initial value:
1 = tsid.TrajectorySE3Constant(
2  "traj-fingertip-position", H_fingertip_ref
3 )

Definition at line 78 of file demo_tsid_talos_gripper_closed_kinematic_chain.py.

◆ trajPosture

demo_tsid_talos_gripper_closed_kinematic_chain.trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)

◆ True

demo_tsid_talos_gripper_closed_kinematic_chain.True

◆ two_pi_f

int demo_tsid_talos_gripper_closed_kinematic_chain.two_pi_f = 2 * np.pi * np.array([0.0, 0.5, 0.0])

◆ two_pi_f_amp

demo_tsid_talos_gripper_closed_kinematic_chain.two_pi_f_amp = np.multiply(two_pi_f, amp)

◆ two_pi_f_squared_amp

demo_tsid_talos_gripper_closed_kinematic_chain.two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)

◆ twoFramesContact_Kp

int demo_tsid_talos_gripper_closed_kinematic_chain.twoFramesContact_Kp = 300

◆ twoFramesContact_w_forceRef

int demo_tsid_talos_gripper_closed_kinematic_chain.twoFramesContact_w_forceRef = 1e-5

◆ urdf

string demo_tsid_talos_gripper_closed_kinematic_chain.urdf = path + "../../tsid_demo_closed_kinematic_chain/urdf/talos_gripper_half.urdf"

◆ v

demo_tsid_talos_gripper_closed_kinematic_chain.v = np.zeros(robot.nv)

◆ v_mean

float demo_tsid_talos_gripper_closed_kinematic_chain.v_mean = v + 0.5 * dt * dv

◆ vector

demo_tsid_talos_gripper_closed_kinematic_chain.vector = pin.StdVec_StdString()

◆ vEE

demo_tsid_talos_gripper_closed_kinematic_chain.vEE = np.zeros(6)

◆ viz

demo_tsid_talos_gripper_closed_kinematic_chain.viz
Initial value:
1 = pin.visualize.MeshcatVisualizer(
2  robot_display.model,
3  robot_display.collision_model,
4  robot_display.visual_model,
5 )

Definition at line 56 of file demo_tsid_talos_gripper_closed_kinematic_chain.py.

◆ w_ee

float demo_tsid_talos_gripper_closed_kinematic_chain.w_ee = 1.0

◆ w_posture

int demo_tsid_talos_gripper_closed_kinematic_chain.w_posture = 1e-3


tsid
Author(s): Andrea Del Prete, Justin Carpentier
autogenerated on Sat May 3 2025 02:48:17