Skip to content
Snippets Groups Projects
Unverified Commit c9c43cdb authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

format detail (#238)

* format detail

* [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci



---------

Co-authored-by: default avatarpre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
parent 7208254b
No related branches found
No related tags found
No related merge requests found
......@@ -3,13 +3,13 @@ Simple demo of usage of ContactTwoFramePositions in TSID
Make the Talos gripper model works with closed kinematic chains
(c) MIPT
"""
import os
import time
import numpy as np
import pinocchio as pin
import tsid
from numpy import nan
from numpy.linalg import norm as norm
np.set_printoptions(precision=3, linewidth=200, suppress=True)
......@@ -111,12 +111,10 @@ invdyn.addRigidContact(
# Setting actuation to zero for passive joints in kinematic chain via TaskActuationBounds
tau_max = model.effortLimit[-robot.na :]
tau_max[
0
] = 0.0 # setting gripper_left_inner_single_joint to passive contstrainig it's actuation bounds to zero
tau_max[
1
] = 0.0 # setting gripper_left_fingertip_3_joint to passive contstrainig it's actuation bounds to zero
# setting gripper_left_inner_single_joint to passive contstrainig it's actuation bounds to zero
tau_max[0] = 0.0
# setting gripper_left_fingertip_3_joint to passive contstrainig it's actuation bounds to zero
tau_max[1] = 0.0
tau_min = -tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
actuationBoundsTask.setBounds(tau_min, tau_max)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment