I’m building a two-segment planar continuum robot in drake. I already write a sdf file with multiple joints working as nodes for continuum robot. Theoretically, these joints will only controlled by the kinematic model (piecewise constant curvature).
That means the value of the joints will only change when a command is given in the forward kinematics, such as plant.SetPositions.
But when I’m using a Simulator and MeshcatVisualizer, the continuum starts to swing, likely affected by the gravity.
So is there a method to only kinetically control the robot and joints without dynamics?
Thanks!
Here is the demo code
meshcat = StartMeshcat()
meshcat.Delete()
meshcat.DeleteAddedControls()
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
Parser(plant, scene_graph).AddModels(file)
plant.WeldFrames(
plant.world_frame(),
plant.GetBodyByName("robot_base").body_frame()
)
plant.Finalize()
MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
plant.SetPositions(plant_context, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
plant.get_actuation_input_port().FixValue(plant_context, np.zeros(10))
simulator = Simulator(diagram, context)
simulator.set_target_realtime_rate(1.0)
meshcat.StartRecording()
simulator.AdvanceTo(10.0 if running_as_notebook else 0.1)
meshcat.StopRecording()
meshcat.PublishRecording()
and this is the sdf file
<?xml version="1.0"?>
<sdf version="1.7">
<model name="two_planar_cc_segments_rev">
<link name="robot_base">
<pose> 0 0 0 0 0 0</pose>
<collision name="robot_base_col">
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</collision>
<visual name="robot_base_vis">
<material>
<diffuse>1.0 0.0 0.0 1.0</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<!-- This is the first CC Segment -->
<link name="s1l0">
<pose relative_to="robot_base"> 0 0 0 0 0 0 </pose>
<collision name="s1l0_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s1l0_vis">
<material>
<diffuse>1.0 1.0 1.0 1.0</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s1q0" type="revolute">
<pose relative_to="robot_base"> 0 0 0 0 0 0</pose>
<parent> robot_base </parent>
<child> s1l0 </child>
<axis>
<xyz> 1 0 0 </xyz>
</axis>
</joint>
<link name="s1l1">
<pose relative_to="s1l0"> 0 0 0.01 0 0 0 </pose>
<collision name="s1l1_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s1l1_vis">
<material>
<diffuse>1.0 1.0 1.0 0.8</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s1q1" type="revolute">
<pose relative_to="s1q0">0 0 0.01 0 0 0 </pose>
<parent> s1l0 </parent>
<child> s1l1 </child>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<link name="s1l2">
<pose relative_to="s1l1"> 0 0 0.01 0 0 0</pose>
<collision name="s1l2_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s1l2_vis">
<material>
<diffuse>1.0 1.0 1.0 0.6</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s1q2" type="revolute">
<pose relative_to="s1q1">0 0 0.01 0 0 0 </pose>
<parent> s1l1 </parent>
<child> s1l2 </child>
<axis>
<xyz>1 0 0</xyz>
<dynamics>
<friction>1</friction>
<damping>1</damping>
</dynamics>
<limit>
<lower>0.0</lower>
<upper>0.1</upper>
</limit>
</axis>
</joint>
<link name="s1l3">
<pose relative_to="s1l2"> 0 0 0.01 0 0 0</pose>
<collision name="s1l3_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s1l3_vis">
<material>
<diffuse>1.0 1.0 1.0 0.4</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s1q3" type="revolute">
<pose relative_to="s1q2">0 0 0.01 0 0 0 </pose>
<parent> s1l2 </parent>
<child> s1l3 </child>
<axis>
<xyz>1 0 0</xyz>
<dynamics>
<friction>1</friction>
<damping>1</damping>
</dynamics>
<limit>
<lower>0.0</lower>
<upper>0.1</upper>
</limit>
</axis>
</joint>
<link name="s1l4">
<pose relative_to="s1l3"> 0 0 0.01 0 0 0</pose>
<collision name="s1l4_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s1l4_vis">
<material>
<diffuse>1.0 1.0 1.0 0.2</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s1q4" type="revolute">
<pose relative_to="s1q3">0 0 0.01 0 0 0 </pose>
<parent> s1l3 </parent>
<child> s1l4 </child>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<frame name="s1-ee-link">
<pose relative_to="s1q4">0 0 0 0 0 0</pose>
</frame>
<!-- Till Here -->
<!-- This is the first CC Segment -->
<link name="s2l0">
<pose relative_to="s1l4"> 0 0 0.01 0 0 0 </pose>
<collision name="s2l0_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s2l0_vis">
<material>
<diffuse> 0.0 1.0 0.0 1.0</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s2q0" type="revolute">
<pose relative_to="s1l4"> 0 0 0.01 0 0 0</pose>
<parent> s1l4 </parent>
<child> s2l0 </child>
<axis>
<xyz> 1 0 0 </xyz>
</axis>
</joint>
<link name="s2l1">
<pose relative_to="s2l0"> 0 0 0.01 0 0 0 </pose>
<collision name="s2l1_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s2l1_vis">
<material>
<diffuse> 0.0 1.0 0.0 0.8</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s2q1" type="revolute">
<pose relative_to="s2q0">0 0 0.01 0 0 0 </pose>
<parent> s2l0 </parent>
<child> s2l1 </child>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<link name="s2l2">
<pose relative_to="s2l1"> 0 0 0.01 0 0 0</pose>
<collision name="s2l2_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s2l2_vis">
<material>
<diffuse>0.0 1.0 0.0 0.6</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s2q2" type="revolute">
<pose relative_to="s2q1">0 0 0.01 0 0 0 </pose>
<parent> s2l1 </parent>
<child> s2l2 </child>
<axis>
<xyz>1 0 0</xyz>
<dynamics>
<friction>1</friction>
<damping>1</damping>
</dynamics>
<limit>
<lower>0.0</lower>
<upper>0.1</upper>
</limit>
</axis>
</joint>
<link name="s2l3">
<pose relative_to="s2l2"> 0 0 0.01 0 0 0</pose>
<collision name="s2l3_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s2l3_vis">
<material>
<diffuse>0.0 1.0 0.0 0.4</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s2q3" type="revolute">
<pose relative_to="s2q2">0 0 0.01 0 0 0 </pose>
<parent> s2l2 </parent>
<child> s2l3 </child>
<axis>
<xyz>1 0 0</xyz>
<dynamics>
<friction>1</friction>
<damping>1</damping>
</dynamics>
<limit>
<lower>0.0</lower>
<upper>0.1</upper>
</limit>
</axis>
</joint>
<link name="s2l4">
<pose relative_to="s2l3"> 0 0 0.01 0 0 0</pose>
<collision name="s2l4_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s2l4_vis">
<material>
<diffuse>0.0 1.0 0.0 0.2</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s2q4" type="revolute">
<pose relative_to="s2q3">0 0 0.01 0 0 0 </pose>
<parent> s2l3 </parent>
<child> s2l4 </child>
<axis>
<xyz>1 0 0</xyz>
</axis>
</joint>
<frame name="s2-ee-link">
<pose relative_to="s2q4">0 0 0.01 0 0 0</pose>
</frame>
<!-- Till Here -->
</model>
</sdf>
I just tried using the simulator = Simulator(diagram, context) to do the simulation and use plant.SetPositions(plant_context, [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) to set the joint values. The plant.SetPositions command just set an initial value for the simulator. I don’t know if there is other methods to do the wanted simulation.
New contributor
Zhiwei is a new contributor to this site. Take care in asking for clarification, commenting, and answering.
Check out our Code of Conduct.